top of page
Search

MPU6050 _ 3D PLANE SIMULATOR

Updated: Oct 10, 2024












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.


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.



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_PI

Gyroscope 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:

 



 

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

 

 

 

·       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

 

 

 

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 )

 

·       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

·       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 :




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

8.1.    Atmel-ice Datasheet

8.2.    ATmega328P Datasheet

8.3.    MPU6050 Datasheet and MPU6050 register map



 

 
 
 

Comments

Subscribe Form

093 149 67 80

©2020 by ASK VINA. Proudly created with Wix.com

bottom of page