Height Measurement Using Gyro Accelerometer

// www.matthewtechub.com
// Calculate Object Height Using MPU6050 Tilt and Laser Module
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

Adafruit_MPU6050 mpu;

// Define the fixed distance between the measuring system and the object (in meters)
const float objectDistance = 20.0; // 20 meters

void setup() {
  Serial.begin(9600);
  while (!Serial)
    delay(10); // Wait for Serial Monitor to open

  // Try to initialize the MPU6050
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  // Set the accelerometer range
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);

  // Set the gyro range
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);

  // Set the filter bandwidth
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  delay(100);
}

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Calculate tilt angle in degrees for Y-axis (vertical tilt)
  float angleY = atan2(-a.acceleration.x, a.acceleration.z) * 180.0 / PI;

  // Normalize angle to a range of 0° to 90° (since we are pointing upward)
  if (angleY < 0) angleY = -angleY; // Take absolute value to make the angle positive

  // Calculate the height of the object using trigonometry
  float angleRad = angleY * PI / 180.0; // Convert angle to radians
  float objectHeight = objectDistance * tan(angleRad); // Calculate height using tangent

  // Print the tilt angle and calculated height
  Serial.print("Tilt Angle Y: ");
  Serial.print(angleY);
  Serial.print("° | Calculated Height: ");
  Serial.print(objectHeight);
  Serial.println(" meters");

  Serial.println("-------------------");
  delay(500);
}