Skip to content

Input devices | Position, Speed and Altitude

This sixth week I worked on getting used to input devices.

For this assignment we had to get data from a sensor and print them on the Serial Monitor.

My inspiration, this time, was the behavior of drones for which the very basic parameters we need to know are:
- Orientation (yaw, pitch and roll).
- Altitude.
- Speed.
Then I made a program that shows on Serial Monitor these three values, taking the data from an Accelerometer and gyroscope, and an ultrasonic sensor.

Software and hardwares

- For this practice we programmed an Arduino UNO with its Arduino IDE.

Components:

For this assignment I used:

Quantity Description
1 x Arduino UNO
1 x MPU 6050 Accelerometer and gyroscope sensor
1 x HC-SR04 Ultrasonic distance sensor
1 x Small breadboard
a few Jumper cables

Circuit

Pin configuration:

- HC-SR04 Ultrasonic distance sensor:

Pin Connection
Vcc 5V
GND GND
Echo Arduino Pin 3
Trigger Arduino Pin 4

Circuit

- MPU-6050 Accelerometer and gyroscope sensor:

Pin Connection
Vcc 5V
GND GND
INT Arduino Pin 2
SCL Arduino Pin A5
SDA Arduino Pin A4

Circuit

Physical circuit:
Circuit

Code

I had as foundation the two example codes for HC-SR04 and MPU-6050, then I re-wrote them for the result I wanted.

If there is something worth highlighting from the coding process is:

  • Since the sensor has its XYZ axis, which are going to be ‘0’ when its in a completely horizontal position, and it’s almost impossible to place it like that, I wrote in the setup a piece of code that takes the position and validates it as ‘0’ values, it also make the system ignore the gravity value in acceleration readings, so then we get the relative acceleration and position values:
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
// Get new sensor events with the readings
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);

offsetax = a.acceleration.x;
offsetay = a.acceleration.y;
offsetaz = a.acceleration.z;

offsetgx = g.gyro.x;
offsetgy = g.gyro.y;
offsetgz = g.gyro.z;
  • Since the gyroscope gives angular velocity in ‘rad/s’ for each axis and I wanted to get yaw/pitch/roll, first I converted ‘rad/s’ to ‘deg/s’, then multiplied it by Time to get ‘deg’, and added them:
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
float yaw = ((g.gyro.x - offsetgx)*180/PI) * 0.1;
float pitch = ((g.gyro.y - offsetgy)*180/PI)  * 0.1;
float roll = ((g.gyro.z - offsetgz)*180/PI) * 0.1;
actyaw = actyaw + yaw;
actpitch = actpitch + pitch;
actroll = actroll + roll;

Serial.print("Yaw: ");
Serial.print(actyaw, 1);
Serial.print(\tPitch: ");
Serial.print(actpitch, 1);
Serial.print(\tRoll: ");
Serial.print(actroll, 1);
Serial.println("º");
  • Since the accelerometer gives acceleration in ‘m/s^2’ for each axis and I wanted to get the horizontal speed, first I multiplied it by Time to get velocity in ‘m/s’, then I calculated horizontal total speed with velocity in X and Y:
1
2
3
4
5
6
7
float velx = (a.acceleration.z - offsetaz)*0.1;
float vely = (a.acceleration.y - offsetay)*0.1;

float hspeed = sqrt((velx*velx)+(vely*vely));
Serial.print("Hor speed: ");
Serial.print(hspeed*100, 2);
Serial.println("cm/s");

Note: for X values I read Z because of the position I put the sensor, so the sensor Z is my world X.

Full code:

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
Adafruit_MPU6050 mpu;
// I used Pi value in calculations
#define PI 3.1415926535

// declare yaw/pitch/roll auxiliar variables
float actyaw = 0, actpitch = 0, actroll = 0;
// declare auxiliar variables for accel offsets
float offsetax = 0, offsetay = 0, offsetaz = 0;
// declare auxiliar variables for gyro offsets
float offsetgx = 0, offsetgy = 0, offsetgz = 0;

// declare variables for sonar sensor
#define echoPin 3
#define trigPin 4
long duration; // variable for the duration of sound wave travel
int distance; // variable for the distance measurement

void setup(void) {
  Serial.begin(115200);
  while (!Serial)
  delay(100); // will pause until serial console is open

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

  // wait for ready
  Serial.println(F("\nPress Enter to begin: "));
  while (Serial.available() && Serial.read()); // empty buffer
  while (!Serial.available());                 // wait for data
  while (Serial.available() && Serial.read()); // empty buffer again

  // sensor parameters setup
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.println("");
  delay(100);

  // Get new sensor events with the readings
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  offsetax = a.acceleration.x;
  offsetay = a.acceleration.y;
  offsetaz = a.acceleration.z;

  offsetgx = g.gyro.x;
  offsetgy = g.gyro.y;
  offsetgz = g.gyro.z;

  // Ultrasonic sensor pins setup
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void loop() {
  // Get new sensor events with the readings
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Calculate yaw/pitch/roll from gyroscope values, and print
  // From the gyroscope we get angular velocity in 'rad/s'
  // First I converted 'rad/s' to 'deg/s', then multiplied it by Time to get 'deg'
  float yaw = ((g.gyro.x - offsetgx)*180/PI) * 0.1;
  float pitch = ((g.gyro.y - offsetgy)*180/PI)  * 0.1;
  float roll = ((g.gyro.z - offsetgz)*180/PI) * 0.1;
  actyaw = actyaw + yaw;
  actpitch = actpitch + pitch;
  actroll = actroll + roll;

  Serial.print("Yaw: ");
  Serial.print(actyaw, 1);
  Serial.print(\tPitch: ");
  Serial.print(actpitch, 1);
  Serial.print(\tRoll: ");
  Serial.print(actroll, 1);
  Serial.println("º");

  // Calculate velocity from accelerometer values
  // From the accelerometer we get acceleration in 'm/s^2'
  // First I multiplied it by Time to get velocity in 'm/s'
  float velx = (a.acceleration.z - offsetaz)*0.1;
  float vely = (a.acceleration.y - offsetay)*0.1;

  // Calculate total horizontal speed, and print
  // First I calculated horizontal total speed with velocity in X and Y
  // I multiplied it by 100 for getting 'cm/s'
  float hspeed = sqrt((velx*velx)+(vely*vely));
  Serial.print("Hor speed: ");
  Serial.print(hspeed*100, 2);
  Serial.println("cm/s");

  // basic loop for HC-SR04 UtraSonic Distance sensor
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  // Calculating the distance
  distance = (duration * 0.034 / 2)-1; // Speed of sound wave divided by 2 (go and back)
  // Displays the distance on the Serial Monitor
  Serial.print("Altitude: ");
  Serial.print(distance);
  Serial.println(" cm\n");

  delay(100);
}

Demonstration

Since the sensor is very sensitive (and my hands shake a lot) we can notice certain changes in the horizontal speed values even when I’m not trying to move horizontally.

Download files

- Official code: SpeedPosAlt.ino
- HC-SR04 example code: HC-SR04_Example.ino
- MPU-6050 example code: MPU-6050_Example.ino


Last update: February 12, 2021