Angle measurement setup

Projects General Guidance
1

Hello everyone, I am currently working on a DIY drone. I have coded the part where it can measure the the Roll, Yaw and Pitch. However, I do not have a setup to move the drone at precise angles and read the angle calculated by the drone. This can be done on only one axis to start with.
What I need to do is: set the drone at a specific angle, record the actual angle, record the angle calculated by the drone and compare them. No, I cannot just place the drone over a wall and using markings to see the angles. I want the drone to continually rotate so I can simulate the effect of linear acceleration on the accelerometer.

My first idea was to have a wooden rod with a bearing in the middle connected to a support frame. On the bearing, I would connect a high quality potentiometer with the rotary part connected to the wooden road. So, when the rod is moved manually, the potentiometer value is changed, and the signal is fed trough a voltage divider to an analog pin. Then, the analog pin is red, ad the analog value is mapped to convert it to the actual angle of the rod. This setup is very difficult to build, I do not discard the idea but I am looking for something simpler.

Another idea I had is to use a high quality servo motor to turn the rod. The issue is that when I give the signal to turn the rod, it will not move to that position instantaneously, therefore, I will not accurately measure the real position of the rod. Maybe it is possible to move the rod by hand and use a code to read the position of the servo motor shaft, I am not sure if there is a servo motor that can return the position of the shaft. I tried using servo.read on a micro servo SG90 connected to Arduino, but it does not work as the value does not change if I spin the shaft by hand.

I am totally open to new approaches to solve my problem. Please provide your precious time to help me prepare the setup.

2

What is the actual problem?

I want the drone to continually rotate so I can simulate the effect of linear acceleration on the accelerometer.

What has rotation to do with "linear acceleration"?

3

Isn't that an off the shelf part?

4

Hello @jremington, when the accelerometer are stationary, the data read is accurate. When the drone would be moving, there would be an extra force acting on the drone apart from gravity. These forces will provide inaccurate accelerometer data. I want to have the drone rotate with the rod, this will apply forces to the accelerometer (which I can detect via software). These forces are conceptually treated as "linear acceleration".

5

Hello @sonofcy , which part is it? if there is something that can do that accurately, it would help me a lot!!!!!

6

Try a cheap rotary table
Drive by hand or with a stepper motor if it must be automated.

7

Talk to people in the RC community.

8

Hello @cedarlakeinstruments , That is a very nice piece of equipment. using a motor will introduce lag between the actual position and the control signal. but I will consider it for future.

9

There are IMUs with built-in sensor fusion algorithms that will provide acceleration-compensated angle data. MPU-6050 is an example. It's obsolete now, but there are newer versions.

10

"Linear acceleration" is an inaccurate and misleading term, loosely defined as acceleration produced by forces other than gravity.

The gyro can be used in sensor fusion algorithms to partially compensate for the errors in yaw, pitch and roll induced by rotation, but cannot help compensate for the errors induced by acceleration due to brakes, motors and the like.

You still haven't explained the actual problem you are trying to solve.

12

Hello @cedarlakeinstruments , that is the exact sensor I am using!!!! Was not aware that it already has a data processing on board feature. Will have a look into that, but it still does not solve the issue of me wanting to verify those angle measurements.

13

Hello @jremington, I am trying to see if the angles calculated by the ESP32-S2 connected to a MPU6050 are accurate. This will then be used used as the algorithm to find orientation for a DIY drone. This is the real problem I am trying to solve, I have no idea if the roll pitch, yaw angles I read are accurate, if not, how much is the error. I hope this gives a more clear idea of what I am trying to do.

14

Here is the current unpolished version of the code, not sure if this matters on not for my problem:

/*
 THIS CODE READS THE MPU6050 AT THE MAX SPEED SUPPORTED 
 PRINTS THE ROLL YAW AND PITCH AS FAST AS POSSIBLE
 THE LOOP FREQUENCY IS LIMITED AT 1 KHZ 
*/

#include <Wire.h>
#include <math.h>

#define LED_BLE_PIN 7
#define LED_RED_PIN 8
#define LED_GRN_PIN 9

const int MPU_ADDR = 0x68; // I2C address of the MPU-6050
const int CONFIG = 0x1A;  // Address of the low pass filter register
float alpha = 0.98;  // Initial complementary filter constant

// Add these variables for offsets
int16_t ax_offset = -340, ay_offset = -130, az_offset = -1140;
int16_t gx_offset = -199, gy_offset = 3, gz_offset = 15;

float roll = 0.0;
float pitch = 0.0;
float yaw = 0.0;

unsigned long lastTime = 0;
unsigned long currentTime = 0;
float dt = 0.001; // Initial dt value for the first loop

float linear_acc_threshold = 0.2; //variable to decide when to use or discard accelerometer data

void setup() {
  pinMode(LED_GRN_PIN, OUTPUT);
  pinMode(LED_BLE_PIN, OUTPUT);
  pinMode(LED_RED_PIN, OUTPUT);

  Serial.begin(921600); // highest speed possible on my setup, higher than this I get errors
  Wire.begin(11, 10); // Initialize I2C with SDA on GPIO11 and SCL on GPIO10
  Wire.setClock(1000000); // Clock speed at 1MHz (highest available)

  // Initialize MPU6050
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0);    // Set to 0 to wake up the MPU-6050
  Wire.endTransmission(true);

  // Set accelerometer sensitivity to ±2g
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x1C); // ACCEL_CONFIG register
  Wire.write(0);    // Set sensitivity to ±2g
  Wire.endTransmission(true);

  // Set gyroscope sensitivity to ±250 degrees/second
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x1B); // GYRO_CONFIG register
  Wire.write(0);    // Set sensitivity to ±250 degrees/second
  Wire.endTransmission(true);

  // Configure the DLPF to 5 Hz
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(CONFIG);
  Wire.write(0x06); // DLPF_CFG = 6 (5 Hz)
  Wire.endTransmission(true);

  // Initial delay to allow the sensor to stabilize
  delay(100);
}

void loop() {
  currentTime = micros();
  dt = (currentTime - lastTime) / 1000000.0; // Convert dt to seconds

  if (dt >= 0.001) { // Ensure the loop runs at approximately 1 kHz (1ms per loop)
    lastTime = currentTime;

    // Read raw values from MPU6050
    int16_t ax, ay, az, gx, gy, gz;
    readMPU6050Data(ax, ay, az, gx, gy, gz);
   
    // Convert raw values to accelerometer (g) and gyroscope (degrees/second)
    // Negative signs to match the orientation of the drone
    float accelX = -ax / 16384.0;
    float accelY = -ay / 16384.0;
    float accelZ = az / 16384.0;
    float gyroX = -gx / 131.0;
    float gyroY = -gy / 131.0;
    float gyroZ = gz / 131.0;
   
    // Calculate roll and pitch from the accelerometer data
    float accelPitch = atan2(accelY, accelZ) * 180 / PI;
    float accelRoll = atan2(-accelX, sqrt(accelY * accelY + accelZ * accelZ)) * 180 / PI;

    // Calculate the magnitude of the acceleration vector
    float accelMagnitude = sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ);

    /*// Integrate gyroscope data to get angles, accounting for cross-coupling effects using small angle approximation
    float rollRad = roll * PI / 180;
    float pitchRad = pitch * PI / 180;

    roll += gyroY * dt + gyroX * rollRad * dt + gyroZ * pitchRad * dt;
    pitch += gyroX * dt - gyroZ * rollRad * dt;
    yaw += (gyroX * rollRad / cos(pitchRad) + gyroZ / cos(pitchRad)) * dt;*/
 
    // Integrate gyroscope data to get angles, accounting for cross-coupling effects witout small angle approximation
    roll += gyroY * dt + gyroX * sin(roll * PI / 180) * tan(pitch * PI / 180) * dt + gyroZ * cos(roll * PI / 180) * tan(pitch * PI / 180) * dt;
    pitch += gyroX * cos(roll * PI / 180) * dt - gyroZ * sin(roll * PI / 180) * dt;
    yaw += (gyroX * sin(roll * PI / 180) / cos(pitch * PI / 180) + gyroZ * cos(roll * PI / 180) / cos(pitch * PI / 180)) * dt;
      
    // Adjust the complementary filter coefficient based on the acceleration magnitude
    if (fabs(accelMagnitude - 1.0) < linear_acc_threshold) {
      // If the acceleration magnitude does NOT deviate significantly from 1g
      // calculate dynamic filter coefficient
      alpha = map(accelMagnitude, 1.0, 1.0 + linear_acc_threshold, 90, 98) / 100.0;
      // Apply complementary filter
      roll = alpha * roll + (1.0 - alpha) * accelRoll;
      pitch = alpha * pitch + (1.0 - alpha) * accelPitch;
      // Yaw is only from the gyroscope since accelerometer doesn't give yaw angle
      //Visualise on the drone the accelerometer data is valid
      digitalWrite(LED_RED_PIN, LOW);
      digitalWrite(LED_GRN_PIN, HIGH);

    } 
    else{
      //Visualise on the drone the accelerometer data is discarded
      digitalWrite(LED_RED_PIN, HIGH);
      digitalWrite(LED_GRN_PIN, LOW);
    }

    // Print the results
    Serial.print("R: ");
    Serial.print(roll);
    Serial.print("\tP: ");
    Serial.print(pitch);
    Serial.print("\tY: ");
    Serial.print(yaw);
    Serial.print("\tF: ");
    Serial.print(1.0 / dt);
    Serial.println(" Hz");
  }
}

void readMPU6050Data(int16_t &ax, int16_t &ay, int16_t &az, int16_t &gx, int16_t &gy, int16_t &gz) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // Starting register for accelerometer readings
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 14, true); // Request 14 bytes

  ax = Wire.read() << 8 | Wire.read() - ax_offset; 
  ay = Wire.read() << 8 | Wire.read() - ay_offset;
  az = Wire.read() << 8 | Wire.read() - az_offset;
  Wire.read(); Wire.read(); // Ignore temperature readings
  gx = Wire.read() << 8 | Wire.read() - gx_offset;
  gy = Wire.read() << 8 | Wire.read() - gy_offset;
  gz = Wire.read() << 8 | Wire.read() - gz_offset;
}
15

The best approach to determine that is to physically set up the sensor at some different known angles, and see if the sensor reproduces those angles.

Note that the MPU-6050 cannot measure yaw, because that requires a North reference. Most people use a 9DOF sensor for that.

To measure pitch and roll in the stationary state, all you need is the accelerometer to provide the Down reference, as you are doing, also described in this tutorial: How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing-DFRobot

16

Hello @jremington yes, what you have said is all correct. Now I want to see the angles while the drone is moving, therefore I do not want to fix it stationary at certain angles and get the reading. I wanna see the calculated readings while the drone is moving along with the real position of the drone.

17

How do you plan to determine the "real position"?

18

@jremington Ideally it has to be a transducer. One idea was to use a potentiometer, when the wooden rod turns, it would turn the pot. I would then always read the analog value and convert it into degrees. i.e. 0V from the voltage divider with the pot could be located at 0 deg, then 2.5V would be 90 deg, then 5V would be 180 deg. It is very easy to design a voltage divider to achieve that and is very easy to map the reading from the analog read to the actual angle (map function itself should be enough to do that).

The idea is that the angle read always comes from the potentiometer which is fixed to the wooden road. No heavy calculation required to find the angle, just an analogread and scaling. This would give a very accurate value, low computationally expensive, extremely reliable. The only issue with this is having the mechanical skills to be able to do it which I am currently lacking. I can do it, but it would take me quite a bit of time and effort.

19

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