How I Developed the Scout Flight Controller, Part 2: Capturing Telemetry with the Gyroscope
This article is part of a series documenting the development of a custom quadcopter flight controller from scratch. For a list of the other related articles, please refer to the introductory article found here.
In the previous chapter of this series, where I discussed the development of the custom flight controller named Scout, we explored how a quadcopter flight controller achieves stable flight. By constantly comparing the actual attitude (or rate of change in attitude) of the quadcopter to the desired attitude (or rate of change in attitude) of the pilot, the flight controller determines the appropriate level of thrust to apply to each motor, thus reaching the desired state. But how does it monitor its own attitude?
A gyroscope is a device that is used to measure angular velocity. In other words, a gyroscope is a module that measures the rate of rotation around all three axes. Gyroscopes are extremely common and can be found in almost every cell phone made today. Gyroscopes also play a critical roll in avionics, and in particular, multicopters.
Rate Mode with the Gyroscope
As mentioned in a previous article, the Scout flight controller implements a Rate Mode — meaning that the quadcopter’s flight controller is exclusively focused on achieving a specific rate of rotation, not orientation angle to the ground. A gyroscope is perfect for this scenario. A flight controller determines motor thrust needed by comparing the actual attitude rate of change to the desired attitude rate of change — a gyroscope allows the flight controller to know one half of this equation.
I mounted an MPU-6050 (on a GY-521 breakout board) onto the quadcopter in the following orientation. The MPU-6050 is a common low-cost gyroscope that is used in many commercial and hobby applications.
With the MPU-6050 lined up along all three axes as seen above, we can visually see how a rotation of the MPU-6050 in any direction can be understood as a change in the roll, pitch, or yaw of the drone. The mounting position and orientation is extremely important. The MPU-6050 must be mounted in the following ways:
- In the center of the drone (where it turns on its axis). Mounting the gyroscope in an area that is not at its center may cause instability, as a true rotation of the drone may be interpretted as an acceleration instead. For the readings to be as “clean” as possible, it must be mounted very near to the center of the drone.
- With the axes of the MPU-6050 lined up as closely as possible with the axes of the quadcopter. Line up the X axis of the gyroscope with the X axis (roll) of the quadcopter, the Y axis with the Y axis (pitch) of the quadcopter, and Z axis with the yaw angle of freedom.
I2C
I2C, or Inter-Integrated Circuit, is a serial communication format invented in the early 1980s that allows for the communication between two devices: the “master”, which is reading from and writing to a peripheral device for its own purposes, and the “slave”, the device itself that is providing a service to the master device.
I2C allows for bidirectional communication between the two devices via only two wires — one for data transmission, known as “SDA”, and one for a clock signal, known as “SCL”, which plays a crucial role in synchronizing the data exchange between the master and slave devices. The Master reads data from and writes data to the Slave device by reading and writing to specific registers of the Slave device.
Fortunately, I2C is a well-understood and implemented communication protocol; the MicroPython implementation for the Raspberry Pi Pico that we are developing in has a full module, machine, for interfacing with peripheral (slave) devices over I2C.
As described in the MPU-6050 datasheet, the MPU-6050 uses the I2C protocol to communicate with its parent (master) device.
Interfacing with the MPU-6050
I’ve put together a MicroPython class that allows for communication with the MPU-6050, and thus, allows for the collection of gyroscope telemetry:
"""
A lightweight MicroPython implementation for interfacing with an MPU-6050 via I2C. Author: Tim Hanewich - https://github.com/TimHanewich Version: 1.0License: MIT License
Copyright 2023 Tim Hanewich
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the “Software”), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
"""import machineclass MPU6050:
"""Class for reading gyro rates and acceleration data from an MPU-6050 module via I2C."""
def __init__(self, i2c:machine.I2C, address:int = 0x68):
"""
Creates a new MPU6050 class for reading gyro rates and acceleration data.
:param i2c: A setup I2C module of the machine module.
:param address: The I2C address of the MPU-6050 you are using (0x68 is the default).
"""
self.address = address
self.i2c = i2c
def wake(self) -> None:
"""Wake up the MPU-6050."""
self.i2c.writeto_mem(self.address, 0x6B, bytes([0x01])) def sleep(self) -> None:
"""Places MPU-6050 in sleep mode (low power consumption). Stops the internal reading of new data. Any calls to get gyro or accel data while in sleep mode will remain unchanged - the data is not being updated internally within the MPU-6050!"""
self.i2c.writeto_mem(self.address, 0x6B, bytes([0x40]))
def who_am_i(self) -> int:
"""Returns the address of the MPU-6050 (ensure it is working)."""
return self.i2c.readfrom_mem(self.address, 0x75, 1)[0]
def read_temperature(self) -> float:
"""Reads the temperature, in celsius, of the onboard temperature sensor of the MPU-6050."""
data = self.i2c.readfrom_mem(self.address, 0x41, 2)
raw_temp:float = self._translate_pair(data[0], data[1])
temp:float = (raw_temp / 340.0) + 36.53
return temp def read_gyro_range(self) -> int:
"""Reads the gyroscope range setting."""
return self._hex_to_index(self.i2c.readfrom_mem(self.address, 0x1B, 1)[0])
def write_gyro_range(self, range:int) -> None:
"""Sets the gyroscope range setting."""
self.i2c.writeto_mem(self.address, 0x1B, bytes([self._index_to_hex(range)]))
def read_gyro_data(self) -> tuple[float, float, float]:
"""Read the gyroscope data, in a (x, y, z) tuple."""
# set the modified based on the gyro range (need to divide to calculate)
gr:int = self.read_gyro_range()
modifier:float = None
if gr == 0:
modifier = 131.0
elif gr == 1:
modifier = 65.5
elif gr == 2:
modifier = 32.8
elif gr == 3:
modifier = 16.4
# read data
data = self.i2c.readfrom_mem(self.address, 0x43, 6) # read 6 bytes (gyro data)
x:float = (self._translate_pair(data[0], data[1])) / modifier
y:float = (self._translate_pair(data[2], data[3])) / modifier
z:float = (self._translate_pair(data[4], data[5])) / modifier
return (x, y, z)
def read_accel_range(self) -> int:
"""Reads the accelerometer range setting."""
return self._hex_to_index(self.i2c.readfrom_mem(self.address, 0x1C, 1)[0])
def write_accel_range(self, range:int) -> None:
"""Sets the gyro accelerometer setting."""
self.i2c.writeto_mem(self.address, 0x1C, bytes([self._index_to_hex(range)]))
def read_accel_data(self) -> tuple[float, float, float]:
"""Read the accelerometer data, in a (x, y, z) tuple."""
# set the modified based on the gyro range (need to divide to calculate)
ar:int = self.read_accel_range()
modifier:float = None
if ar == 0:
modifier = 16384.0
elif ar == 1:
modifier = 8192.0
elif ar == 2:
modifier = 4096.0
elif ar == 3:
modifier = 2048.0
# read data
data = self.i2c.readfrom_mem(self.address, 0x3B, 6) # read 6 bytes (accel data)
x:float = (self._translate_pair(data[0], data[1])) / modifier
y:float = (self._translate_pair(data[2], data[3])) / modifier
z:float = (self._translate_pair(data[4], data[5])) / modifier
return (x, y, z)
def read_lpf_range(self) -> int:
return self.i2c.readfrom_mem(self.address, 0x1A, 1)[0]
def write_lpf_range(self, range:int) -> None:
"""
Sets low pass filter range.
:param range: Low pass range setting, 0-6. 0 = minimum filter, 6 = maximum filter.
""" # check range
if range < 0 or range > 6:
raise Exception("Range '" + str(range) + "' is not a valid low pass filter setting.")
self.i2c.writeto_mem(self.address, 0x1A, bytes([range]))
#### UTILITY FUNCTIONS BELOW ####
def _translate_pair(self, high:int, low:int) -> int:
"""Converts a byte pair to a usable value. Borrowed from https://github.com/m-rtijn/mpu6050/blob/0626053a5e1182f4951b78b8326691a9223a5f7d/mpu6050/mpu6050.py#L76C39-L76C39."""
value = (high << 8) + low
if value >= 0x8000:
value = -((65535 - value) + 1)
return value def _hex_to_index(self, range:int) -> int:
"""Converts a hexadecimal range setting to an integer (index), 0-3. This is used for both the gyroscope and accelerometer ranges."""
if range== 0x00:
return 0
elif range == 0x08:
return 1
elif range == 0x10:
return 2
elif range == 0x18:
return 3
else:
raise Exception("Found unknown gyro range setting '" + str(range) + "'")
def _index_to_hex(self, index:int) -> int:
"""Converts an index integer (0-3) to a hexadecimal range setting. This is used for both the gyroscope and accelerometer ranges."""
if index == 0:
return 0x00
elif index == 1:
return 0x08
elif index == 2:
return 0x10
elif index == 3:
return 0x18
else:
raise Exception("Range index '" + index + "' invalid. Must be 0-3.")
Don’t be too concerned with the minute details of using I2C within the code above! Understanding how to use the class above is plenty sufficient for this project.
An example of using the MPU6050 class seen above:
import machinei2c = machine.I2C(0, sda=machine.Pin(12), scl=machine.Pin(13)) # creating the object that allows for I2C communication in MicroPythonimu = MPU6050(i2c) # passing the i2c object to the MPU6050 class above. This class will handle all communicationsimu.wake() # wakes up the MPU-6050 (it may have been in sleep mode)gyro_data = imu.read_gyro_data()print(gyro_data) # (0.346823, -0.198345, 0.023958)
Low Pass Filtering
The MPU-6050 is very sensitive. Naturally, when the motors begin rotating, there are minor vibrations from the four motors and propellers. While we cannot see them with our eyes, the MPU-6050’s gyroscope picks them up. This can be interpreted as erratic changes in attitude rate of change and thus can be hugely inhibiting to stable flight.
Fortunatlely, the MPU-6050 has a built-in low pass filter for this type of scenario. The onboard low pass filter filters out low-end frequencies, like those created by the motors/propellers. By turning this low pass filter setting on, we can get a much cleaner stream of incoming telemetry that better represents the attitude rate of change of the vehicle (it is not perfect, but vastly improved).
To activate the MPU-6050’s onboard digital low pass filter (extending from the chunk of code above):
imu.write_lpf_range(5) # Turning the low pass filter setting to level 5 (out of 6)Accounting for Gyroscope Bias
Every gyroscope exhibits a phenomenon known as “bias”. When leaving the gyroscope perfectly still and running several consecutive readings, you will notice that no gyroscope will ever read exactly 0.0, 0.0, 0.0, indicating a rotational velocity of zero in each axis. Instead, consistent readings of seemingly random values are obtained, averaging around readings like 0.346823, -0.198345, 0.023958, or even varying further in certain scenarios.
This inherent deviation is referred to as “bias”. Despite the gyroscope remaining perfectly still, there will still be at least some level of indicated rotation. No gyroscope is perfect, and bias exists for several reasons — imperfections in manufacturing, temperature variations, residual stress, electromagnetic interference, aging, etc.
Obviously we cannot pass these raw values to the quadcopter flight controller as this would lead to the flight controller to believe that the vehicle is constantly undergoing a slight rotation, despite it remaining motionless. To rectify this issue, we must account for gyroscope bias with a calibration.
The calibration process is conceptually simple: upon system boot-up, we dedicate a few seconds to monitor the raw gyroscope readings while the quadcopter remains perfectly stationary. We then calculate the average of these observed values across all three axes. In all subsequent gyroscope readings, we will subtract each axis’ respective calibration reading (the average we calculated) to get the “calibrated” reading; essentially, we are subtracting the bias out of the raw reading. This adjustment effectively eliminates the bias from the raw readings, providing us with a “clean” reading that can be utilized within our flight controller code. Details of this calibration process will be explained in a future chapter.
Using the Gyroscope Telemetry
By reading data from the MPU-6050 gyroscope via the I2C protocol, the Scout flight controller now possesses the ability to monitor its own change in attitude. Now what do we do with this data?
This data — the quadcopters actual rate of rotation must be compared against the pilots desired rate of rotation. Now that we have one half of the equation, we must acquire the other: the pilots desired rate of change in attitude.
In the next chapter you’ll learn how an onboard FS-iA6B radio receiver allows Scout to receive attitude adjustment commands from the pilot via a radio transmitter. This is the final piece of data the flight controller requires before it can determine what levels of thrust to apply to each motor.
