Characteristics of the Project
Using Atmel Studio:
Provides a higher level of control over the hardware. It supports low-level programming and direct interaction with the microcontroller's registers, allowing developers to optimize performance and fully utilize all features of the microcontroller.
Requires direct programming with registers and No Library
You can use tools like Atmel-ICE or AVRISP mkII to debug and upload programs. This allows direct programming into the flash memory without needing a bootloader.
Much more flexible as it supports a wide range of AVR, ARM microcontrollers and allows deeper custom configurations of the hardware.
1. Introduction
1.1 Overview about MPU6050
What is MPU6050 :
The MPU6050 is a widely used 6-axis motion tracking sensor that integrates a 3-axis accelerometer and a 3-axis gyroscope on a single chip. This combination allows the MPU6050 to measure both linear acceleration (using the accelerometer) and angular velocity (using the gyroscope) along the X, Y, and Z axes. It is often used in applications requiring motion detection, orientation sensing, or stabilization, such as in drones, robots, smartphones, gaming controllers, and wearable devices.
Read more in the link : https://components101.com/sensors/mpu6050-module
1.2 Overview about ATmega328P
What is Atmega328P :
The ATmega328P is an 8-bit microcontroller from Atmel (now owned by Microchip Technology) that is widely known for its use in the popular Arduino Uno board. It belongs to the AVR family of microcontrollers, which are based on a RISC (Reduced Instruction Set Computer) architecture. This microcontroller is favored in embedded systems due to its balance of power, performance, and simplicity.
Read More in the link :
1.3 Overview about Programmer Atmel-Ice
The Atmel-ICE is a powerful in-circuit debugger and programmer designed for Atmel (now Microchip) microcontrollers, including AVR, SAM (ARM-based), and ATmega devices. It is widely used in both professional development and hobbyist projects for debugging and programming microcontrollers, offering a variety of features for working with embedded systems.
Read more in the datasheet link : https://ww1.microchip.com/downloads/en/DeviceDoc/Atmel-ICE_UserGuide.pdf
1.4 Purpose of Plane Simulation
A plane simulation using the ATmega328P and MPU6050 serves as a practical project to visualize and test motion-tracking and control systems in embedded development. It has applications in flight simulation, drone control, education, and DIY projects, allowing users to experiment with real-time orientation sensing and control without needing a physical aircraft.
1. Motion Tracking and Real-Time Control:
· MPU6050: The MPU6050 provides data on angular velocity (gyroscope) and linear acceleration (accelerometer) along three axes (X, Y, Z). This data can be used to track the orientation and movement of the simulated plane.
· ATmega328P: The ATmega328P microcontroller processes this data and transmits it to a computer, where it controls the simulated plane’s behavior in real time. It serves as the brain of the system, handling the communication between the sensor and the simulation software
2. Embedded Systems Development:
· For students or engineers, such a project provides hands-on experience with embedded systems, combining sensors (MPU6050), microcontrollers (ATmega328P), and programming (Arduino or Atmel Studio).
· The goal may be to practice collecting sensor data, processing it with embedded software, and integrating it with external systems like simulations or visual displays.
3. Simulation for Algorithm Testing:
· It allows developers to experiment with flight control algorithms without needing a physical aircraft or drone. The simulation helps test how the plane responds to certain commands or environmental changes (e.g., wind or turns) based on sensor feedback.
· It’s an effective way to simulate autonomous flight control systems, which can later be implemented in real-world drones or UAVs.
4. Educational and Research Applications:
· Educational Tool: This type of project is often used in educational settings to teach students about sensor fusion, real-time data processing, and embedded systems.
· Research Applications: It may also serve as a research project in areas like robotics, control systems, or flight dynamics, where accurate real-time simulation is critical for algorithm development and testing.
2. How MPU6050 works :
Reference :
2.1. How Does MEMS Accelerometer Work?
MEMS accelerometers are used wherever there is a need to measure linear motion, either movement, shock, or vibration but without a fixed reference. They measure the linear acceleration of whatever they are attached to. All accelerometers work on the principle of a mass on a spring, when the thing they are attached to accelerates, then the mass wants to remain stationary due to its inertia and therefore the spring is stretched or compressed, creating a force which is detected and corresponds to the applied acceleration. If you wanna learn more about MEMS sensors you can check out the linked article.
2.2. Address of MPU6050
The slave address of the MPU-60X0 is b110100X which is 7bits long. The LSB bit of the 7bit address is determined by the logic level on pin AD0. This allows two MPU-60X0s to be connected to the same I2C bus. When used in this configuration, the address of the one of the devices should be b1101000 (pin AD0 is logic low) and the address of the other should be b1101001 (pin AD0 is logic high).
Ex) b1101000: 0x68 (pin AD0: logic low)
1) Slave Address: 0x68, [0x68 + W]: b1101000 + 0 => b11010000: 0xD0 => TWDR = 0xD0
2) Slave Address: 0x68, [0x68 + R]: b1101000 + 1 => b11010001: 0xD1 => TWDR = 0xD1
Ex) b1101001: 0x69 (pin AD0: logic high)
1) Slave Address: 0x69, [0x69 + W]: b1101001 + 0 => b11010010: 0xD2 => TWDR = 0xD2
2) Slave address: 0x69, [0x69 + R]: b1101001 + 1 => b11010011: 0xD3 => TWDR = 0xD3
2.3. How Does MEMS Accelerometer and Gyroscope Work?
The MEMS Gyroscope working is based on the Coriolis Effect. The Coriolis Effect states that when a mass moves in a particular direction with velocity and an external angular motion is applied to it, a force is generated and that causes a perpendicular displacement of the mass. The force that is generated is called the Coriolis Force and this phenomenon is known as the Coriolis Effect. The rate of displacement will be directly related to the angular motion applied.
The MEMS Gyroscope contains a set of four proof mass and is kept in a continuous oscillating movement. When an angular motion is applied, the Coriolis Effect causes a change in capacitance between the masses depending on the axis of the angular movement. This change in capacitance is sensed and then converted into a reading. Here is a small animation showing the movement of these proof masses on the application of an angular movement for different axis.
2.4. How to calculate Angle using Accelerometer :
· Roll: Rotation around the X-axis (tilting left or right).
· Pitch: Rotation around the Y-axis (tilting forward or backward).
· Yaw: Rotation around the Z-axis (turning left or right)
Roll and pitch can be calculated by using the accelerometer data, then we will combine that data with data of gyroscopes in “complememtary filter”. The formulas are:
Accelerometer data :
float accel_angle_x = atan2(accel_y,sqrt(accel_x*accel_x+accel_z*accel_z))*180/ M_P;
float accel_angle_y = atan2(-accel_x,sqrt(accel_y*accel_y+accel_z*accel_z))*180/M_PIGyroscope Data :
float gyro_angle_x = gyro_x*deltaTime;
float gyro_angle_y = gyro_y*deltaTime;
float gyro_angle_z = gyro_z*deltaTime;
Complementary Filter :
angle_x=angle_x + gyro_angle_x;
angle_y=angle_y + gyro_angle_y;
// Apply complementary filter ( 98% OF GYROSCOPE AND 2% OF ACCELEROMETER )
angle_x = ALPHA angle_x + (1 - ALPHA) accel_angle_x;
angle_y = ALPHA angle_y + (1 - ALPHA) accel_angle_y;
angle_z=angle_z + gyro_angle_z;
We can’t calculate angle of Z-axis by using accelerometer data beacause :
· An accelerometer measures linear acceleration and gravitational forces acting on it. It can only provide information about the direction of gravity and the linear acceleration of the sensor in space.
· Yaw Measurement: Yaw is the angle of rotation around the vertical (Z) axis, which involves circular motion rather than linear. Accelerometers do not measure rotation; they measure the effect of gravitational pull and any linear acceleration.
3. Hardware Setup
3.1. Connect MPU6050 to ATmega328P
Protocol : I2c
Required Pin :
· GND (Ground) in MPU6050 and
Arduino UNO
· VCC (Voltage Supply) in MPU6050 and
5v in Arduino UNO
· SCL (Serial Clock) in MPU6050 and A5
in Arduino UNO
· SDA (Serial Data) in MPU6050 and A4 in Arduino UNO
3.2. Connect Programmer Amtel-ice to ATmega328P
Required cable : ISCP cable
Check the ICSP cable 6-pin 100-mil connector of the Atmel-ice with the Arduino pin connector
by looking at the architecture diagram of the Arduino Uno Rev3 and the Manuals of the Atmel-
ice debbuger:
Reference for arduino : https://www.arduino.cc/en/uploads/Main/Arduino_Uno_Rev3-schematic.pdf
Reference for atmel-ice: https://ww1.microchip.com/downloads/aemDocuments/documents/OTH/ProductDocuments/UserGuides/Atmel-ICE_UserGuide.pdf
Result :
4.3. Configuring Arduino hardware
DebugWIRE Interface is a debugging protocol for Atmel (now Microchip) 8-bit AVR microcontrollers, designed to work with minimal pin usage. This protocol uses the RESET pin of the microcontroller for debugging, enabling access to features like pause, run, set breakpoints, and read/write memory data.
Advantages of DebugWIRE on Arduino Uno (ATmega328P):
· Uses only the RESET pin: The protocol requires only one pin (RESET) for debugging, saving I/O pins compared to JTAG or SPI.
· Full debugging support: It allows pausing, running the program, setting breakpoints, and checking memory data directly on the microcontroller without needing extra hardware.
· Real-time debugging: DebugWIRE can monitor the program's operation in re al-time, which is helpful for developing and troubleshooting embedded code.
· Integration with Atmel Studio: DebugWIRE works well with Atmel Studio, allowing easy debugging directly in this integrated development environment (IDE).
Disadvantages of DebugWIRE on Arduino Uno:
· RESET pin is unavailable: When using DebugWIRE, the RESET pin is occupied for debugging and cannot be used for normal RESET functionality. This can be inconvenient in some cases where manual resets are needed.
· Lacks advanced JTAG features: While DebugWIRE is convenient, it lacks some advanced features that JTAG provides, such as enhanced hardware inspection or complex breakpoint management.
· Limited compatibility: DebugWIRE is only available on Atmel 8-bit AVR microcontrollers like the ATmega328P, so it is not compatible with ARM microcontrollers or other architectures.
· Must be disabled to program via ICSP: After enabling DebugWIRE, you must disable it if you want to reprogram the chip using the standard ICSP interface, and this requires tools like the Atmel-ICE.
Because of Limitations of debugWIRE so if you want to use DebugWIRE on an Arduino Uno (which uses the ATmega328P microcontroller), you need to carefully manage the fact that the DebugWIRE interface shares the same pin as the RESET pin. Here's 2 wat you can use :
Disable Auto-Reset on Arduino Uno:
o Arduino Uno boards are designed with an auto-reset feature, meaning the reset pin is used during every upload to restart the microcontroller. When using DebugWIRE, this feature conflicts with the debugging process because DebugWIRE also needs control over the reset pin.
o To disable auto-reset, you can:
§ Place a capacitor (typically 10 µF) between the RESET pin and GND.
§ Alternatively, you can cut the "RESET-EN" trace on the bo
References : https://ww1.microchip.com/downloads/en/DeviceDoc/Atmel-7810-Automotive-Microcontrollers-ATmega328P_Datasheet.pdf
· page 221 : debugWIRE On-chip Debug System
· page 222 : Limitations of debugWIRE ( pay attention here )
( cut Reset EN in Aruidno UNO )
4. Software Setup
4.1. Install Atmel Studio 7
4.2. Install Processing for simulating Plane
4.3. Install Arduino IDE ( if you don’t have Programer Atmel-Ice to use )
4.4. Configuring setting in Atmel studio
a. Setting for Atmel-Ice :
· Access to Project > ‘Project name’ properties > Tool
· In Selected debugger/programmer area, choose Atmel-Ice
· In interface area , choose dubugWire
b. Setting for float data sending :
· Access to Project > ‘Project name’ properties > Tool Chain > AVR/GNU Linker > Miscellaneous
· Write “-Wl,-u,vfprintf -lprintf_flt -lm” to Other Linker Flags Area
c. Setting for Optimization :
· Access to Project > ‘Project name’ properties > Tool Chain > Optimization
· In Optimization Level , choose option None( -O0 )
· Purpose : Watching Assembler during debugging
5. Peripheral Programming
Manual/Datasheet/PDF : http://airlab.inje.ac.kr/2024/dsmc2/twi.pdf
5.1. Atmega328P Communication with MPU6050
5.1.1. Overview about TWI (I2C ):
· I²C (Inter-Integrated Circuit), pronounced I-squared-C, is a multi-master, multi-slave, single-ended, serial computer bus.
· TWI (Two Wire Interface) is essentially the same bus implemented on various system-onchip processors from Atmel
5.1.2. Start and Stop Conditions in TWI (I2C ):
· START condition: A HIGH to LOW transition of the SDA line while SCL is HIGH.
· STOP condition: A LOW to HIGH transition of the SDA line while SCL is HIGH.
· START and STOP conditions are always generated by the master.
· Between a START and a STOP condition, the bus is considered busy, and no other master should try to seize control of the bus.
5.1.3. Transfer Modes
The TWI interface may be configured to operate as a master and/or a slave.
At any particular time, the interface will be operating in one of the following modes:
· Master Transmitter
· Master Receiver
· Slave Transmitter
· Slave Receiver
5.1.4. Initializing TWI (I2C )
Manual/Datasheet : https://cdn.sparkfun.com/assets/c/a/8/e/4/Atmel-42735-8-bit-AVR-Microcontroller-ATmega328-328P_Datasheet.pdf
· Page 270 - 288 : Modes in TWI / I2C
· Page 292 : Register TWBR
· Page 293 : Register TWSR
· Page 295 : Register TWDR
· Page 296 : Register TWCR
· I2C.h :
#define SCL_CLOCK 100000L // I2C clock speed 100kHz
#define F_CPU 16000000UL // Clock speed of CPU
#include <avr/io.h>
inline void I2C_init(void);
uint8_t i2c_start(void);
uint8_t i2c_write(uint8_t data);
uint8_t i2c_read_ack(void);
uint8_t i2c_read_nack(void);
void i2c_stop(void);
void I2C_Start_Wait(char slave_write_address);
uint8_t I2C_Repeated_Start(char slave_read_address);· I2C.c :
#include "I2C.h"
void I2C_init(void) {
TWSR = 0x00; // 0x00 Set prescaler to 1 (TWPS = 0)
// 0x01 prescalser to 4
// 0x02 prescaler to 16
// 0x03 prescaler to 64
TWBR = ((F_CPU / SCL_CLOCK) - 16) / 2; // Set bit rate
}
// manual page 275 and 278 Atmega328/P
// start condition and stop condition in iic.pdf page 8
void I2C_Start_Wait(char slave_write_address) /* I2C start wait function */
{
/*
TWCR Register page 296 ATmega328-328P_Datasheet.pdf
Bit 7 Bit 6 Bit 5 Bit 4 Bit 3 Bit 2 Bit 1 Bit 0
TWINT TWEA TWSTA TWSTO TWWC TWEN – TWIE
*/
uint8_t status; /* Declare variable */
while (1)
{
TWCR = (1<<TWSTA)|(1<<TWEN)|(1<<TWINT); // 1<<TWEN Enable TWI
// 1<<TWSTA generate start condition
// 1<<TWINT clear interrupt flag by writting 1 to TWCRn.TWINT, now it's 0
while (!(TWCR & (1<<TWINT))); // Wait until TWI finish its current job (start condition)
/* After finish a job, the hardware will automatically
change TWINT bit to 1 to stop the loop */
/*
TWSR Register page 293 ATmega328-328P_Datasheet.pdf
Bit 7 Bit 6 Bit 5 Bit 4 Bit 3 Bit 2 Bit 1 Bit 0
TWS7 TWS6 TWS5 TWS4 TWS3 – TWPS1 TWPS0
*/
status = TWSR & 0xF8; // Read TWI status register with masking lower three bits
if (status != 0x08) // Check if start condition transmitted successfully or not?
continue; // If no then continue with start loop again
TWDR = slave_write_address; // If yes then write SLA+W in TWI data register
TWCR = (1<<TWEN)|(1<<TWINT); // Enable TWI and clear interrupt flag
while (!(TWCR & (1<<TWINT))); /* Wait until TWI finish its current job (Write operation) */
status = TWSR & 0xF8; /* Read TWI status register with masking( remove ) lower three bits */
if (status != 0x18 ) /* Check weather SLA+W transmitted & ack received or not? */
{
i2c_stop(); /* If not then generate stop condition */
continue; /* continue with start loop again */
}
break; /* If yes then break loop */
}
}
uint8_t i2c_write(uint8_t data) {
// page 295 ATmega328-328P_Datasheet.pdf
TWDR = data; // Load data into data register
TWCR = (1 << TWEN) | (1 << TWINT); // Start transmission
while (!(TWCR & (1 << TWINT))); // Wait for transmission to complete
uint8_t status = TWSR & 0xF8; // Check status 1111 1000
if (status == 0x28)
return 0; // SLA+W transmitted, ACK ( slave ) received and slave ready to receive more data
else if (status == 0x30)
{
i2c_stop(); // SLA+W transmitted, NACK ( slave ) received, slave is busy and don't want to receive more data
return 1;
} else
return 2;
} // Other
uint8_t i2c_read_ack(void) {
TWCR = (1 << TWEN) | (1 << TWINT) | (1 << TWEA); // Enable ACK
while (!(TWCR & (1 << TWINT))); // Wait for data to be received
return TWDR; // Return received data
}
uint8_t i2c_read_nack(void) {
TWCR = (1 << TWEN) | (1 << TWINT); // Disable ACK
while (!(TWCR & (1 << TWINT))); // Wait for data to be received
return TWDR; // Return received data
}
void i2c_stop(void) {
TWCR = (1 << TWSTO) | (1 << TWEN) | (1 << TWINT); // Send STOP condition
while (TWCR & (1 << TWSTO)); // Wait for STOP condition to be executed
}
//Manual page 289
uint8_t I2C_Repeated_Start(char slave_read_address) /* I2C repeated start function */
{
uint8_t status; /* Declare variable */
TWCR = (1<<TWSTA)|(1<<TWEN)|(1<<TWINT); /* Enable TWI, generate start condition and clear interrupt flag */
while (!(TWCR & (1<<TWINT))); /* Wait until TWI finish its current job (start condition) */
status = TWSR & 0xF8; /* Read TWI status register with masking lower three bits */
if (status != 0x10) /* Check weather repeated start condition transmitted successfully or not? */
return 0; /* If no then return 0 to indicate repeated start condition fail */
TWDR = slave_read_address; /* If yes then write SLA+R in TWI data register */
TWCR = (1<<TWEN)|(1<<TWINT); /* Enable TWI and clear interrupt flag */
while (!(TWCR & (1<<TWINT))); /* Wait until TWI finish its current job (Write operation) */
status = TWSR & 0xF8; /* Read TWI status register with masking lower three bits */
if (status == 0x40) /* Check weather SLA+R transmitted & ack received or not? */
return 1; /* If yes then return 1 to indicate ack received */
if (status == 0x48) /* Check weather SLA+R transmitted & nack received or not? */
{
i2c_stop();
return 2;
} /* If yes then return 2 to indicate nack received i.e. device is busy */
else
return 3; /* Else return 3 to indicate SLA+W failed */
}5.1.5. Reading Data from MPU6050
Manual/DataSheet :
· Page 14 : GYRO_CONFIG
· Page 15 : ACCEL_CONFIG
· Page 29 : ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H, ACCEL_YOUT_L, ACCEL_ZOUT_H, and ACCEL_ZOUT_L
· Page 29, 31 : Setting Accel and gyro sensitivity
· Page 40 : Register PWR_MGMT_1
· MPU6050.h :
#include "MPU6050.h"
#include "I2C.h"
float gyro_x,gyro_y,gyro_z,accel_x,accel_y,accel_z,temper;
int16_t gyro_x_offset, gyro_y_offset,gyro_z_offset,accel_x_offset,accel_y_offset, accel_z_offset;
void mpu6050_init() {
I2C_Start_Wait(0xD0); // Write address MPU6050 : 0XD0
i2c_write(MPU6050_PWR_MGMT_1); // Power management register
i2c_write(0x00); // Set to 0 to wake up MPU6050
i2c_stop();
set_gyro_sensitivity(0x18); // 0x00 is ±250 deg/s and sensitivity is 131 LSB/deg/s
// 0x08 is ±500 deg/s and sensitivity is 65.5 LSB/deg/s
// 0x10 is ±1000 deg/s and sensitivity is 32.8 LSB/deg/s
// 0x18 is ±2000 deg/s and sensitivity is 16.4 LSB/deg/s
set_accel_sensitivity(0x00); // 0x00 is ±2g and sensitivity is 16384 LSB/g
// 0x08 is ±4g and sensitivity is 8192 LSB/g
// 0x10 is ±8g and sensitivity is 4096 LSB/g
// 0x18 is ±16g and sensitivity is 2048 LSB/g
}
void set_gyro_sensitivity(uint8_t sensitivity) {
I2C_Start_Wait(0xD0); // Write address MPU6050 : 7 bit address MPU6050 and a bit W(0) or R(1)
i2c_write(MPU6050_GYRO_CONFIG); // Gyroscope configuration register
i2c_write(sensitivity); // Set sensitivity
i2c_stop();
}
void set_accel_sensitivity(uint8_t sensitivity) {
I2C_Start_Wait(0xD0); // Write address MPU6050 : 0XD0
i2c_write(MPU6050_ACCEL_CONFIG); // Accelerometer configuration register
i2c_write(sensitivity); // Set sensitivity
i2c_stop();
}
void MPU_Start_Loc()
{
I2C_Start_Wait(0xD0); // Write address MPU6050 : 0XD0
i2c_write(ACCEL_XOUT_H); // Write start location address from where to read */
I2C_Repeated_Start(0xD1); // I2C start with device read address */
}
void Read_RawValue()
{
MPU_Start_Loc(); /* Read Gyro values , check in manuals in
page 7 MPU-6000-Register-Map.pdf */
accel_x = (((int)i2c_read_ack()<<8) | (int)i2c_read_ack());
accel_y = (((int)i2c_read_ack()<<8) | (int)i2c_read_ack());
accel_z = (((int)i2c_read_ack()<<8) | (int)i2c_read_ack());
temper = (((int)i2c_read_ack()<<8) | (int)i2c_read_ack());
gyro_x = (((int)i2c_read_ack()<<8) | (int)i2c_read_ack());
gyro_y = (((int)i2c_read_ack()<<8) | (int)i2c_read_ack());
gyro_z = (((int)i2c_read_ack()<<8) | (int)i2c_read_nack());
i2c_stop();
}5.1.6. Initializing UART
Manual/Datasheet : https://cdn.sparkfun.com/assets/c/a/8/e/4/Atmel-42735-8-bit-AVR-Microcontroller-ATmega328-328P_Datasheet.pdf
· Page 230 : Example code about UART
· Page 245 : Register UCSR0A
· Page 249 : Register UCSR0C
· Page 252 : Register UBRR0L
· Page 253 : Register UBRR0H
· UART.h :
#ifndef UART_H
#define UART_H
void UART_Init(unsigned int ubrr);
void UART_Transmit(unsigned char data);
void UART_Print(const char* str);
#endif // UART_H· UART.c :
#include "uart.h"
#include <avr/io.h>
#include <stdio.h>
#include <stdlib.h>
// Manual Atmel-7810-Automotive-Microcontrollers-ATmega328P_Datasheet.pdf
void UART_Init(unsigned int ubrr) {
UBRR0H = (unsigned char)(ubrr >> 8); //1000 0000 ubrr 104 : 1101000 >>8 = 0000 0000
UBRR0L = (unsigned char)ubrr; // only use low UBRR0L
UCSR0B = (1 << RXEN0) | (1 << TXEN0); // Enable receiver and transmitter , manual page 160
UCSR0C = (1 << UCSZ01) | (1 << UCSZ00); // 8-bit data format , manual page 162
}
void UART_Transmit(unsigned char data) {
while (!(UCSR0A & (1 << UDRE0))); // Wait for empty transmit buffer, if UDRE0 is 1 , so it's empty. Manual page 159
UDR0 = data; // Transmit data
}
void UART_Print(const char* str) {
while (*str) {
while (!(UCSR0A & (1 << UDRE0))); // Wait for empty transmit buffer
UDR0 = *str++;
}
}5.1.7. Initializing Timer
· Timer.h :
#include <avr/interrupt.h>
void timer0_init() ;
ISR(TIMER0_COMPA_vect) ;
unsigned long millis() ;· Timer.c :
#include "Timer.h"
volatile unsigned long milliseconds = 0; // ¹Ð¸®ÃÊ Ä«¿îÅÍ
void timer0_init() {
//setting for 2 register is TCCR0A and TCCR0B
/*
TCCR0A
Bit 7 Bit 6 Bit 5 Bit 4 Bit 3 Bit 2 Bit 1 Bit 0
COM0A1 COM0A0 COM0B1 COM0B0 – – WGM01 WGM00
TCCR0B
Bit 7 Bit 6 Bit 5 Bit 4 Bit 3 Bit 2 Bit 1 Bit 0
FOC0A FOC0B – – WGM02 CS02 CS01 CS00
*/
TCCR0A |= (1 << WGM01); // Sets Timer0 to CTC mode. Manual page 140 about modes in atmega328/P
TCCR0B |= (1 << CS01) | (1 << CS00); // Sets the prescaler to 64, page 142 manual
OCR0A = 249; // Output Compare Register A (OCR0A) to 249. Timer frequency of 1 kHz (1 millisecond period).
// T= prescaler × (OCR0A+1)/fCPU
TIMSK0 |= (1 << OCIE0A); // Enables the Output Compare A Match interrupt every time counter reach to value of OCR0A
sei(); // Enables global interrupt
}
ISR(TIMER0_COMPA_vect) {
milliseconds++; // triggered every time Timer0 reaches the value in OCR0A
}
unsigned long millis() {
unsigned long ms;
cli(); // temporarily disable interrupts
ms = milliseconds;
sei(); // re-enable interrupts
return ms;
}5.1. Processing Sensor Data
5.2.1. Using Calibration :
Reference : https://mjwhite8119.github.io/Robots/mpu6050
void Calibration() {
char buffer[256] ;
Mean_RawData();
accel_x_offset = Mean_Ax/8;
accel_y_offset = Mean_Ay/8;
accel_z_offset = (Mean_Az-16384)/8;
gyro_x_offset = Mean_Gx/4;
gyro_y_offset = Mean_Gy/4;
gyro_z_offset = Mean_Gz/4;
while (1){
int ready=0;
Mean_RawData();
Mean_Ax = Mean_Ax - accel_x_offset;
Mean_Ay = Mean_Ay - accel_y_offset;
Mean_Az = Mean_Az - accel_z_offset;
Mean_Gx = Mean_Gx - gyro_x_offset;
Mean_Gy = Mean_Gy - gyro_y_offset;
Mean_Gz = Mean_Gz - gyro_z_offset;
snprintf(buffer, sizeof(buffer), "%.2f %.2f %.2f %.2f %.2f %.2f\n", Mean_Ax,Mean_Ay,Mean_Az , Mean_Gx,Mean_Gy,Mean_Gz);
UART_Print(buffer);
// 0, 0, 16384, 0, 0, 0 Error Tolerance ready ok.
if (Mean_Ax <= 8&&Mean_Ax >= -8) {ready++;
UART_Print("1 dung");}
else accel_x_offset = accel_x_offset + Mean_Ax/Acc_ErrTol;
if (Mean_Ay<=8&&Mean_Ay>=-8) {ready++;
UART_Print("2 dung");}
else accel_y_offset = accel_y_offset + Mean_Ay/Acc_ErrTol;
if (16384-Mean_Az <= 8&&16384-Mean_Az >= -8){ready++;
UART_Print("3 dung");}
else accel_z_offset = accel_z_offset + (Mean_Az - 16384)/Acc_ErrTol;
if (Mean_Gx <= 1&&Mean_Gx >= -1) {ready++;
UART_Print("4 dung");}
else gyro_x_offset = gyro_x_offset + Mean_Gx/(Gyro_ErrTol+1);
if (Mean_Gy <= 1&&Mean_Gy >= -1) {ready++;
UART_Print("5 dung");}
else gyro_y_offset = gyro_y_offset + Mean_Gy/(Gyro_ErrTol+1);
if (Mean_Gz <= 1&&Mean_Gz>=-1){ready++;
UART_Print("6 dung");}
else gyro_z_offset = gyro_z_offset + Mean_Gz/(Gyro_ErrTol+1);
UART_Print("\n");
if (ready==6) break;
}
}
void Mean_RawData()
{
int i=0;
long Buf_Ax=0, Buf_Ay=0, Buf_Az=0, Buf_Gx=0, Buf_Gy=0, Buf_Gz=0;
for (i=0; i < (num_readings + 100); i++) {
Read_RawValue();
if (i >= 100) { /* First 100 data are discarded */
Buf_Ax = Buf_Ax + accel_x;
Buf_Ay = Buf_Ay + accel_y;
Buf_Az = Buf_Az + accel_z;
Buf_Gx = Buf_Gx + gyro_x;
Buf_Gy = Buf_Gy + gyro_y;
Buf_Gz = Buf_Gz + gyro_z;
}
}
Mean_Ax = Buf_Ax / num_readings ;
Mean_Ay = Buf_Ay / num_readings;
Mean_Az = Buf_Az / num_readings;
Mean_Gx = Buf_Gx / num_readings;
Mean_Gy = Buf_Gy / num_readings;
Mean_Gz = Buf_Gz / num_readings;
}Purpose: The calibrate() function is used to determine the offsets (bias values) for both the gyroscope and accelerometer by averaging a large number of raw sensor readings.
How It Works:
· It takes 1000 readings from both the gyroscope and accelerometer.
· The readings are summed for each axis.
· The offsets (average bias) for each axis are calculated by dividing the sums by the number of readings.
· For the Z-axis of the accelerometer, an additional correction is applied to account for gravity.
· It will take you abotu 1 minute to measure the data for calibration .
5.2.2. Converting Raw Data to Angle Values
Read_RawValue();
gyro_x = ((gyro_x)-gyro_x_offset/4) / GYRO_SENSITIVITY;
gyro_y = ((gyro_y)-gyro_y_offset/4) / GYRO_SENSITIVITY;
gyro_z = ((gyro_z)-gyro_z_offset/4) / GYRO_SENSITIVITY;
accel_x = ((accel_x)-accel_x_offset/8) / ACCEL_SENSITIVITY;
accel_y = ((accel_y)-accel_y_offset/8) / ACCEL_SENSITIVITY;
accel_z = (accel_z-accel_y_offset/8) / ACCEL_SENSITIVITY;5.2.3. Apply Complementary filter
A Complementary Filter is a simple and efficient sensor fusion technique used to combine data from two different sensors (usually a gyroscope and an accelerometer) to obtain a more accurate and stable estimate of orientation or angle. It is commonly used in applications like quadcopters, mobile robots, and other systems that need reliable orientation tracking.
Why Use the Complementary Filter?
The complementary filter helps overcome the individual limitations of the gyroscope and accelerometer:
· The gyroscope provides smooth and accurate readings over short intervals, but it drifts over time.
· The accelerometer provides stable readings over long periods but is noisy and less reliable for quick changes.
By combining the strengths of both, the complementary filter gives you a stable and accurate angle measurement.
Steps of Applying a Complementary Filter:
1. Initialize the Filter:
· Set an initial angle, usually from the accelerometer because it provides an absolute angle reference.
2. Get Sensor Readings:
· Read the angular velocity (gyro_angle_rate) from the gyroscope.
· Read the orientation angle (accelerometer_angle) from the accelerometer.
3. Calculate the New Angle Using the Complementary Filter Formula:
· Use the formula to combine the gyroscope's angle change and the accelerometer's orientation angle to get the updated angle.
4. Update and Repeat:
· Repeat the process at regular intervals (based on the loop frequency or Δt) to continuously update the angle estimate.
float accel_angle_x = atan2(accel_y, sqrt(accel_x*accel_x+accel_z*accel_z))*180/ M_PI ;
float accel_angle_y = atan2(-accel_x, sqrt(accel_y*accel_y+accel_z*accel_z)) *180/M_PI;
angle_x=angle_x + gyro_x*deltaTime;
angle_y=angle_y + gyro_y*deltaTime;
//if(gyro_z<1.5&&gyro_z>-1.5)
//gyro_z=0;
angle_z=angle_z+gyro_z*deltaTime;
// Apply complementary filter
angle_x = ALPHA * angle_x + (1 - ALPHA) * accel_angle_x;
angle_y = ALPHA * angle_y + (1 - ALPHA) * accel_angle_y;5.2.4. Sending data via UART
// Format data as CSV
snprintf(buffer, sizeof(buffer), "%.2f %.2f %.2f\n", angle_x,angle_y,angle_z);
// Send formatted data
UART_Print(buffer);5.2.5. Summary Code :
#include "Timer.h"
#include "MPU6050.h"
#include "I2C.h"
#include "UART.h"
#include <math.h>
#define BAUD 9600
#define MY_UBRR F_CPU/16/BAUD-1
#define ALPHA 0.96
#define GYRO_SENSITIVITY 16.4 // ±250 dps sensitivity
#define ACCEL_SENSITIVITY 16384.0 // ±2g sensitivity
int16_t gyro_x_offset = 0, gyro_y_offset = 0,gyro_z_offset = 0,accel_x_offset = 0,accel_y_offset = 0, accel_z_offset = 0;
extern float gyro_x,gyro_y,gyro_z,accel_x,accel_y,accel_z,temper;
float angle_x = 0.0;
float angle_y = 0.0;
float angle_z = 0.0;
void calibrate() {
int num_readings = 1000;
long gyro_x_sum = 0, gyro_y_sum = 0, gyro_z_sum = 0;
long accel_x_sum = 0, accel_y_sum = 0, accel_z_sum = 0;
for (int i = 0; i < num_readings; i++) {
Read_RawValue();
gyro_x_sum += gyro_x;
gyro_y_sum += gyro_y;
gyro_z_sum += gyro_z;
accel_x_sum += accel_x;
accel_y_sum += accel_y;
accel_z_sum += accel_z;
_delay_ms(100); // delay to get correct data
}
// average value ( mean )
gyro_x_offset = gyro_x_sum / num_readings;
gyro_y_offset = gyro_y_sum / num_readings;
gyro_z_offset = gyro_z_sum / num_readings;
accel_x_offset = accel_x_sum / num_readings;
accel_y_offset = accel_y_sum / num_readings;
accel_z_offset = accel_z_sum / num_readings - 16384; // 1g is equivalent to 16384 because setting for accel sensitivity is ±2g
}
void loop() {
float deltaTime = 0, currentTime = 0.0, previousTime = 0.0;
char buffer[256];
calibrate();
while (1) {
previousTime = currentTime; // Previous time is stored before the actual time read
currentTime = millis(); // Current time actual time read
deltaTime = (currentTime - previousTime) / 1000;
// Read gyro data
// Apply offset correction
Read_RawValue();
gyro_x = ((gyro_x)-gyro_x_offset/4) / GYRO_SENSITIVITY;
gyro_y = ((gyro_y)-gyro_y_offset/4) / GYRO_SENSITIVITY;
gyro_z = ((gyro_z)-gyro_z_offset/4) / GYRO_SENSITIVITY;
accel_x = ((accel_x)-accel_x_offset/8) / ACCEL_SENSITIVITY;
accel_y = ((accel_y)-accel_y_offset/8) / ACCEL_SENSITIVITY;
accel_z = (accel_z-accel_y_offset/8) / ACCEL_SENSITIVITY;
// Calculate angle from accelerometer
float accel_angle_x = atan2(accel_y, sqrt(accel_x*accel_x+accel_z*accel_z))*180/ M_PI ;
float accel_angle_y = atan2(-accel_x, sqrt(accel_y*accel_y+accel_z*accel_z)) *180/M_PI;
angle_x=angle_x + gyro_x*deltaTime;
angle_y=angle_y + gyro_y*deltaTime;
//if(gyro_z<1.5&&gyro_z>-1.5)
//gyro_z=0;
angle_z=angle_z+gyro_z*deltaTime;
// Apply complementary filter
angle_x = ALPHA * angle_x + (1 - ALPHA) * accel_angle_x;
angle_y = ALPHA * angle_y + (1 - ALPHA) * accel_angle_y;
//angle_z = ALPHA * angle_z + (1 - ALPHA) * accel_angle_z;
//angle_z=angle_z+gyro_z*deltaTime;
if(previousTime==0.0)
continue;
// Format data as CSV
snprintf(buffer, sizeof(buffer), "%.2f %.2f %.2f\n", angle_x,angle_y,angle_z);
// Send formatted data
UART_Print(buffer);
// _delay_ms(100); // Adjust delay as needed
}
}
void setup() {
I2C_init(); // Initialize I2C
_delay_ms(20);
mpu6050_init(); // Initialize MPU6050
_delay_ms(20);
UART_Init(MY_UBRR); // Initialize UART
_delay_ms(20);
timer0_init();
}
int main(void) {
setup();
loop();
return 0;
}6. 3D Plane Simulation
6.1. Configuring Port and Baud rate
void setup() {
// Set up the window and OpenGL for 3D rendering
size(600, 600, P3D);
lights();
smooth();
// Get the first available port
port = new Serial(this, "myPort", “baudrate” );
// Send a single character to trigger sensor communication
port.write('r');
}
In the fuction ‘ port = new Serial(this, "myPort", 9600); ’, you need to change
· myPort ( Com6, Com7 ,… ) to be suitable for your Arduino
· baudrate ( 9600, 16200,…. ) to be suitable for your setting
1.2. Receiving Data from Microcontroller
We will use Processing to get data sending via UART
void serialEvent(Serial port) {
// Read the incoming serial data
String data = port.readStringUntil('\n');
if (data != null) {
data = trim(data); // Remove extra spaces or newlines
String[] values = split(data, ' '); // Split the string by spaces
if (values.length == 3) {
// Parse pitch, roll, yaw values
roll = float(values[0]);
pitch = float(values[1]);
yaw = float(values[2]);
}
}
}· In this code you will format your data to be suitable for data format that you have sent using UART
· String[] values = split(data, ' '); // Split the string by spaces
· Like in this example, my format data will be ‘ AngleX AngleY AngleZ ’, so each data will be separated by a space ‘ ’, so I will use function split to separate them
6.3. Updating Rotation Angles (Pitch, Roll, Yaw)
6.3.1. Convert data from Degree to Radian
// Apply the rotations based on sensor data
// Rotate around Y-axis (roll) first, which aligns with the length of the plane
rotateY(radians(yaw));
// Rotate around X-axis (pitch)
rotateX(radians(pitch));
// Rotate around Z-axis (yaw)
rotateZ(radians(roll));
6.3.2. Applying Data and Displaying Plane Simulation in Processing
// Draw the main body (red box) which is aligned with the Y-axis
fill(255, 0, 0, 200);
box(100, 50, 200);
// Draw front-facing tip (blue cylinder)
fill(0, 0, 255, 200);
pushMatrix();
translate(0, 0, -100);
rotateX(HALF_PI);
popMatrix();
// Draw wings and tail fin (green)
fill(0, 255, 0, 200);
// Left wing
pushMatrix();
translate(-60, 0, 0);
rotateZ(HALF_PI);
box(20, 100, 20);
popMatrix();
// Right wing
pushMatrix();
translate(60, 0, 0);
rotateZ(-HALF_PI);
box(20, 100, 20);
popMatrix();
popMatrix();6.4. Summary Code :
import processing.serial.*;
Serial port; // The serial port
float roll, pitch, yaw;
void setup() {
// Set up the window and OpenGL for 3D rendering
size(600, 600, P3D);
lights();
smooth();
// Get the first available port
String portName = Serial.list()[0];
port = new Serial(this, "COM9", 9600);
// Send a single character to trigger sensor communication
port.write('r');
}
void draw() {
// Black background
background(0);
// Translate to the center of the screen (plane stays at center)
pushMatrix();
translate(width / 2, height / 2, 0);
// Apply the rotations based on sensor data
// Rotate around Y-axis (roll) first, which aligns with the length of the plane
rotateY(radians(yaw));
// Rotate around X-axis (pitch)
rotateX(radians(pitch));
// Rotate around Z-axis (yaw)
rotateZ(radians(roll));
// Draw the main body (red box) which is aligned with the Y-axis
fill(255, 0, 0, 200);
box(100, 50, 200);
// Draw front-facing tip (blue cylinder)
fill(0, 0, 255, 200);
pushMatrix();
translate(0, 0, -100);
rotateX(HALF_PI);
popMatrix();
// Draw wings and tail fin (green)
fill(0, 255, 0, 200);
// Left wing
pushMatrix();
translate(-60, 0, 0);
rotateZ(HALF_PI);
box(20, 100, 20);
popMatrix();
// Right wing
pushMatrix();
translate(60, 0, 0);
rotateZ(-HALF_PI);
box(20, 100, 20);
popMatrix();
popMatrix();
// Display sensor data on the screen
fill(255);
text("Roll: " + roll, 10, height - 60);
text("Pitch: " + pitch, 10, height - 45);
text("Yaw: " + yaw, 10, height - 30);
}
void serialEvent(Serial port) {
// Read the incoming serial data
String data = port.readStringUntil('\n');
if (data != null) {
data = trim(data); // Remove extra spaces or newlines
String[] values = split(data, ' '); // Split the string by spaces
if (values.length == 3) {
// Parse pitch, roll, yaw values
roll = float(values[0]);
pitch = float(values[1]);
yaw = float(values[2]);
}
}
}7. Troubleshoting :
7.1. Problem with programer :
If you don’t have programer Atmel-Ice , you can use external tool to program the Arduino via bootloader
Step by step in the link youtube : https://www.youtube.com/watch?v=zEbSQaQJvHI
7.2. Problem with float data sending via UART
If you viewed data via UART that sent by MPU6050 in Data Visualizer and saw the data just showing only “ ? “ symbol , so it means there was a problem with function snprintf when sending float data
Solution : Access to Project > ‘Project name’ properties > Tool Chain > AVR/GNU Linker > Miscellaneous
Write “-Wl,-u,vfprintf -lprintf_flt -lm” to Other Linker Flags
Result :
8. References
Comments