Gyro L3GD20H problem. Atmega 328p

Support Sensors
1

Hello, I have a problem with reading data from Gyro. I check firts 8 from 16 bits on LCD(X,Y,Z axis). It shows, I think, not expected values . I would like to see angles. Could you please help me?

[code]/*

  • main.c
  • Created on: 7 gru 2015
  •  Author: PC

*/

#include <avr/pgmspace.h>
#include <avr/eeprom.h>
#include <avr/io.h>
#include <avr/wdt.h>
#include <util/delay.h>
#include <avr/power.h>
#include "zyroskop.h"
#include "lcd.h"

int main()
{
int Gx,Gy,Gz;
int rate_x =0, rate_y =0,rate_z =0;
float off_x = 0, off_z = 0, off_y = 0;
float gain = 1;
//int16_t Gx1 = 0,Gx2 = 0,Gy1 = 0,Gy2 = 0,Gz1 = 0,Gz2 = 0;

int Gx1 = 0,Gx2 = 0,Gy1 = 0,Gy2 = 0,Gz1 = 0,Gz2 = 0;




LCD_Initalize();
LCD_Clear();

Inicjacja();
initgyro();

while(1)
 {
      Gx1 = TWI_read_register(gyroscope_W ,OUT_X_L);
      Gx2 = TWI_read_register(gyroscope_W ,OUT_X_H);

      Gy1 = TWI_read_register(gyroscope_W ,OUT_Y_L);
      Gy2 = TWI_read_register(gyroscope_W ,OUT_Y_H);

      Gz1 = TWI_read_register(gyroscope_W ,OUT_Z_L);
      Gz2 = TWI_read_register(gyroscope_W ,OUT_Z_H);

      Gx = (Gx2 << 8);
      Gy = (Gy2 << 8);//int 16 i int to to samo
      Gz = (Gz2 << 8);

      rate_x = (int)(((float)(Gx2 << 8)-off_x) *gain);
      rate_y = (int)(((float)(Gy2 << 8)-off_y) *gain);
      rate_z = (int)(((float)(Gz2 << 8)-off_z) *gain);

      LCD_GoTo(0,0);
      lcd_int(Gx2);

      LCD_GoTo(0,1);
      lcd_int(Gz2);

      LCD_GoTo(8,0);
      lcd_int(Gy2);
/*	_delay_ms(500);
	p -= 1;
	u -= 1;
	q -= 1;
	LCD_GoTo(0,0);
	lcd_int(q);
	LCD_GoTo(7,0);
	lcd_int((int)p);
	LCD_GoTo(0,1);
	lcd_int((int)u);*/
 }

}
[/code]

[code]/*

  • lcd.h
  • Created on: 19-04-2015
  •  Author: PC

*/

#ifndef LCD_H_
#define LCD_H_

//-------------------------------------------------------------------------------------------------
// Wyświetlacz alfanumeryczny ze sterownikiem HD44780
// Sterowanie w trybie 4-bitowym bez odczytu flagi zajętości
// z dowolnym przypisaniem sygnałów sterujących
// Plik : HD44780.h
// Mikrokontroler : Atmel AVR
// Kompilator : avr-gcc
// Autor : Radosław Kwiecień
// Źródło : http://radzio.dxp.pl/hd44780/
// Data : 24.03.2007
//-------------------------------------------------------------------------------------------------

#include <avr/io.h>
#include <util/delay.h>

//-------------------------------------------------------------------------------------------------
//
// Konfiguracja sygnałów sterujących wyświetlaczem.
// Można zmienić stosownie do potrzeb.
//
//-------------------------------------------------------------------------------------------------
#define LCD_RS_DIR DDRD
#define LCD_RS_PORT PORTD
#define LCD_RS (1 << PD6) //PD6 //RW podlaczyc do GND!!!

#define LCD_E_DIR DDRD
#define LCD_E_PORT PORTD
#define LCD_E (1 << PD5) //PD5

#define LCD_DB4_DIR DDRB
#define LCD_DB4_PORT PORTB
#define LCD_DB4 (1 << PB4) //PB4

#define LCD_DB5_DIR DDRB
#define LCD_DB5_PORT PORTB
#define LCD_DB5 (1 << PB3) //PB3

#define LCD_DB6_DIR DDRB
#define LCD_DB6_PORT PORTB
#define LCD_DB6 (1 << PB2) //PB2

#define LCD_DB7_DIR DDRB
#define LCD_DB7_PORT PORTB
#define LCD_DB7 (1 << PB1) //PB1

//-------------------------------------------------------------------------------------------------
//
// Instrukcje kontrolera Hitachi HD44780
//
//-------------------------------------------------------------------------------------------------

#define HD44780_CLEAR 0x01

#define HD44780_HOME 0x02

#define HD44780_ENTRY_MODE 0x04
#define HD44780_EM_SHIFT_CURSOR 0
#define HD44780_EM_SHIFT_DISPLAY 1
#define HD44780_EM_DECREMENT 0
#define HD44780_EM_INCREMENT 2

#define HD44780_DISPLAY_ONOFF 0x08
#define HD44780_DISPLAY_OFF 0
#define HD44780_DISPLAY_ON 4
#define HD44780_CURSOR_OFF 0
#define HD44780_CURSOR_ON 2
#define HD44780_CURSOR_NOBLINK 0
#define HD44780_CURSOR_BLINK 1

#define HD44780_DISPLAY_CURSOR_SHIFT 0x10
#define HD44780_SHIFT_CURSOR 0
#define HD44780_SHIFT_DISPLAY 8
#define HD44780_SHIFT_LEFT 0
#define HD44780_SHIFT_RIGHT 4

#define HD44780_FUNCTION_SET 0x20
#define HD44780_FONT5x7 0
#define HD44780_FONT5x10 4
#define HD44780_ONE_LINE 0
#define HD44780_TWO_LINE 8
#define HD44780_4_BIT 0
#define HD44780_8_BIT 16

#define HD44780_CGRAM_SET 0x40

#define HD44780_DDRAM_SET 0x80

//-------------------------------------------------------------------------------------------------
//
// Deklaracje funkcji
//
//-------------------------------------------------------------------------------------------------

void LCD_WriteCommand(unsigned char);
void LCD_WriteData(unsigned char);
void LCD_WriteText(char *);
void LCD_GoTo(unsigned char, unsigned char);
void LCD_Clear(void);
void LCD_Home(void);
void LCD_Initalize(void);
void lcd_int(int val);
void lcd_str(char * str);
void lcd_char(char c);
//-------------------------------------------------------------------------------------------------
//
// Koniec pliku HD44780.h
//
//-------------------------------------------------------------------------------------------------

#endif /* LCD_H_ */
[/code]

[code]/*

  • lcd.c
  • Created on: 19-04-2015
  •  Author: PC

*/

//-------------------------------------------------------------------------------------------------
// Wyświetlacz alfanumeryczny ze sterownikiem HD44780
// Sterowanie w trybie 4-bitowym bez odczytu flagi zajętości
// z dowolnym przypisaniem sygnałów sterujących
// Plik : HD44780.
// Mikrokontroler : Atmel AVR
// Kompilator : avr-gcc
// Autor : Radosław Kwiecień
// Źródło : http://radzio.dxp.pl/hd44780/
// Data : 24.03.2007
//-------------------------------------------------------------------------------------------------

#include “lcd.h”
//-------------------------------------------------------------------------------------------------
//
// Funkcja wystawiająca półbajt na magistralę danych
//
//-------------------------------------------------------------------------------------------------
void _LCD_OutNibble(unsigned char nibbleToWrite)
{
if(nibbleToWrite & 0x01)
LCD_DB4_PORT |= LCD_DB4;
else
LCD_DB4_PORT &= ~LCD_DB4;

if(nibbleToWrite & 0x02)
LCD_DB5_PORT |= LCD_DB5;
else
LCD_DB5_PORT &= ~LCD_DB5;

if(nibbleToWrite & 0x04)
LCD_DB6_PORT |= LCD_DB6;
else
LCD_DB6_PORT &= ~LCD_DB6;

if(nibbleToWrite & 0x08)
LCD_DB7_PORT |= LCD_DB7;
else
LCD_DB7_PORT &= ~LCD_DB7;
}
//-------------------------------------------------------------------------------------------------
//
// Funkcja zapisu bajtu do wyświetacza (bez rozróżnienia instrukcja/dane).
//
//-------------------------------------------------------------------------------------------------
void _LCD_Write(unsigned char dataToWrite)
{
LCD_E_PORT |= LCD_E;
_LCD_OutNibble(dataToWrite >> 4);
LCD_E_PORT &= ~LCD_E;
LCD_E_PORT |= LCD_E;
_LCD_OutNibble(dataToWrite);
LCD_E_PORT &= ~LCD_E;
_delay_us(50);
}
//-------------------------------------------------------------------------------------------------
//
// Funkcja zapisu rozkazu do wyświetlacza
//
//-------------------------------------------------------------------------------------------------
void LCD_WriteCommand(unsigned char commandToWrite)
{
LCD_RS_PORT &= ~LCD_RS;
_LCD_Write(commandToWrite);
}
//-------------------------------------------------------------------------------------------------
//
// Funkcja zapisu danych do pamięci wyświetlacza
//
//-------------------------------------------------------------------------------------------------
void LCD_WriteData(unsigned char dataToWrite)
{
LCD_RS_PORT |= LCD_RS;
_LCD_Write(dataToWrite);
}
//-------------------------------------------------------------------------------------------------
//
// Funkcja wyświetlenia napisu na wyswietlaczu.
//
//-------------------------------------------------------------------------------------------------
void LCD_WriteText(char * text)
{
while(*text)
LCD_WriteData(*text++);
}
//-------------------------------------------------------------------------------------------------
//
// Funkcja ustawienia współrzędnych ekranowych
//
//-------------------------------------------------------------------------------------------------
void LCD_GoTo(unsigned char x, unsigned char y)
{
LCD_WriteCommand(HD44780_DDRAM_SET | (x + (0x40 * y)));
}
//-------------------------------------------------------------------------------------------------
//
// Funkcja czyszczenia ekranu wyświetlacza.
//
//-------------------------------------------------------------------------------------------------
void LCD_Clear(void)
{
LCD_WriteCommand(HD44780_CLEAR);
_delay_ms(2);
}
//-------------------------------------------------------------------------------------------------
//
// Funkcja przywrócenia początkowych współrzędnych wyświetlacza.
//
//-------------------------------------------------------------------------------------------------
void LCD_Home(void)
{
LCD_WriteCommand(HD44780_HOME);
_delay_ms(2);
}
//-------------------------------------------------------------------------------------------------
//
// Procedura inicjalizacji kontrolera HD44780.
//
//-------------------------------------------------------------------------------------------------

void lcd_char(char c)
{
LCD_WriteData( ( c>=0x80 && c<=0x87 ) ? (c & 0x07) : c);
}

void lcd_str(char * str)
{
register char znak;
while ( (znak=*(str++)) ) lcd_char( znak );
}

void lcd_int(int val)
{
char bufor[17];
lcd_str( itoa(val, bufor, 10) );
}

void LCD_Initalize(void)
{
unsigned char i;
LCD_DB4_DIR |= LCD_DB4; // Konfiguracja kierunku pracy wyprowadzeń
LCD_DB5_DIR |= LCD_DB5; //
LCD_DB6_DIR |= LCD_DB6; //
LCD_DB7_DIR |= LCD_DB7; //
LCD_E_DIR |= LCD_E; //
LCD_RS_DIR |= LCD_RS; //
_delay_ms(15); // oczekiwanie na ustalibizowanie się napiecia zasilajacego
LCD_RS_PORT &= ~LCD_RS; // wyzerowanie linii RS
LCD_E_PORT &= ~LCD_E; // wyzerowanie linii E

for(i = 0; i < 3; i++) // trzykrotne powtórzenie bloku instrukcji
{
LCD_E_PORT |= LCD_E; // E = 1
_LCD_OutNibble(0x03); // tryb 8-bitowy
LCD_E_PORT &= ~LCD_E; // E = 0
_delay_ms(5); // czekaj 5ms
}

LCD_E_PORT |= LCD_E; // E = 1
_LCD_OutNibble(0x02); // tryb 4-bitowy
LCD_E_PORT &= ~LCD_E; // E = 0

_delay_ms(1); // czekaj 1ms
LCD_WriteCommand(HD44780_FUNCTION_SET | HD44780_FONT5x7 | HD44780_TWO_LINE | HD44780_4_BIT); // interfejs 4-bity, 2-linie, znak 5x7
LCD_WriteCommand(HD44780_DISPLAY_ONOFF | HD44780_DISPLAY_OFF); // wyłączenie wyswietlacza
LCD_WriteCommand(HD44780_CLEAR); // czyszczenie zawartosći pamieci DDRAM
_delay_ms(2);
LCD_WriteCommand(HD44780_ENTRY_MODE | HD44780_EM_SHIFT_CURSOR | HD44780_EM_INCREMENT);// inkrementaja adresu i przesuwanie kursora
LCD_WriteCommand(HD44780_DISPLAY_ONOFF | HD44780_DISPLAY_ON | HD44780_CURSOR_OFF | HD44780_CURSOR_NOBLINK); // włącz LCD, bez kursora i mrugania
}

//-------------------------------------------------------------------------------------------------
//
// Koniec pliku HD44780.c
//
//-------------------------------------------------------------------------------------------------
[/code]

[code]/*

  • zyroskop.c
  • Created on: 7 gru 2015
  •  Author: PC

*/

/*

  • Akcelerometr1.c
  • Created on: 18-08-2015
  •  Author: PC

*/

#include "zyroskop.h"
#include <avr/io.h>
#include <avr/wdt.h>
#include <util/delay.h>
#include <avr/power.h>


void Inicjacja(void)
{
        TWCR &= ~(1<<TWIE);
        TWBR = 16;
        TWSR &= ~(1<<TWPS1) | (1<<TWPS0);
}

void Start(void)
{
        TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWSTA);
        while(!(TWCR & (1<<TWINT)));
}

void Write(uint8_t address)
{
        TWDR = address;
        TWCR = (1<<TWINT)|(1<<TWEN);
        while(!(TWCR&(1<<TWINT)));
}

void Read(void)
{
        uint8_t zmienna=0;
        zmienna = TWDR;
        TWCR = (1<<TWINT)|(1<<TWEN);
        while(!(TWCR&(1<<TWINT)));
}

void Stop(void)
{
        TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWSTO);
}

void TWI_write_register(uint8_t address, uint8_t reg, uint8_t val)
{

Start(); // Send Start
Write(address); // Send IIC “Write” Address
Write(reg); // Send Register
Write(val); // Send Value
Stop(); // Send Stop
}

uint8_t TWI_read_register(uint8_t address, uint8_t reg)
{
Start();
Write(address); //adres slave
//pisanie adresu rejestru
Write(reg); //adres rejestru
Start();
Write(address+1); //adres slave+1

        Read();

        Stop();
		return TWDR;

}

void initgyro(void)
{
TWI_write_register(gyroscope_W,0x39,0x0b00000101);
TWI_write_register(gyroscope_W,CTRL1,0x4F);
TWI_write_register(gyroscope_W,CTRL4,0b00010000);
}

[/code]

[code]/*

  • zyroskop.h
  • Created on: 7 gru 2015
  •  Author: PC

*/

#include <avr/io.h>

#ifndef ZYROSKOP_H_
#define ZYROSKOP_H_

#define WHO_AM_I 0x0F
#define gyroscope_W 0xD6
#define gyroscope_R 0xD7 // SA0 to + VDD through 10kOhm resistor, read

#define CTRL1 0x20
#define CTRL1REG 0x0F // 12,5 Hz (ODR), n.a. (cut-off[hz]), power mode normal, all axis enabled
#define CTRL2 0x21

#define CTRL3 0x22
#define CTRL4 0x23

#define CTRL5 0x24
#define CTRL6 0x25

#define OUT_X_L 0x28
#define OUT_X_H 0x29
#define OUT_Y_L 0x2A
#define OUT_Y_H 0x2B
#define OUT_Z_L 0x2C
#define OUT_Z_H 0x2D

#define FIFO_CTRL 0x2E
#define FIFO_SRC 0x2F

void initgyro(void);
void Inicjacja(void);
void Start(void);
void Write (uint8_t address);
void Read(void);
void Stop(void);
void TWI_write_register(uint8_t address, uint8_t reg, uint8_t val);
uint8_t TWI_read_register(uint8_t address, uint8_t reg);

#endif /* ZYROSKOP_H_ */
[/code]

2

The L3GD20H is a rate gyro. It does not return an angle, it returns the rate of rotation. See the data sheet for the details.

Why are you ignoring the low 8 bits of the measurement on each axis?

3

Thank you for answer.

I know that it does not return angle - but i still think that data are wrong. I will try to change it for an angle. I am not sure if my code is correct and if my gyro is 100% good.

I ignore low 8 bits, because it changes very fast and at first I want to have correct data, then try to do something with whole 16 bits.

4

The proper way to test if the data are correct is to mount the gyro on a turntable, rotating at a known rate, and compare the known rate with the measured rate. However, first subtract the offsets (the output value on each axis, when the gyro is not rotating).

Do not forget that there is a scale factor, described in the data sheet, that converts the value returned into a rate in degrees/second.

5

Thank you for advice :wink:

I also have another problem.

I set:

          rate_x = (Gx2 << 8)+Gx1;
          rate_y = (Gy2 << 8)+Gy1;
          rate_z = (Gz2 << 8)+Gz1;

          LCD_GoTo(0,0);
          lcd_int(rate_x);

          LCD_GoTo(0,1);
          lcd_int(rate_z);

          LCD_GoTo(8,0);
          lcd_int(rate_y);

Good thing is that when I change position (for example rotate X axis) values change with a good sign(plus or minus)

But there is something strange. When I run program and do not move Gyro - my offset X value is changing all the time. I got 450401-600401 offset value set on LCD. Strange fact is that 3 last numbers are not moving, but three first are changing(example values 450401;500401;540401; 492401…). Do you have idea why?

6

[quote]Do you have idea why?[/quote]You forgot to erase the LCD screen between print operations.

7

Thank you very much for answer.

What do you mean by erasing LCD between print operation? You think about something like function -> LCD_Clear, which erase entire surface of lcd? If you do, than can you explain how to use it between prints in such a way to avoid erasing everything? When i tried to put LCD_Clear function after compile, my lcd remained blank

8

Print one or more blanks after a number, so that you erase what was left on the screen from the last time.

I’ve forgotten to do that many times, and there is always a moment of confusion when the numbers on the screen make no sense.

9

Thanks. I used delay:

LCD_Clear();

           LCD_GoTo(0,0);
                    lcd_int(rate_x);

                    LCD_GoTo(0,1);
                    lcd_int(rate_z);

                    LCD_GoTo(8,0);
                    lcd_int(rate_y);

          _delay_ms(100); // lub mniej
    /*	_delay_ms(500);

Now I have to change it into angle value. I have a question to you. You said "Do not forget that there is a scale factor, described in the data sheet, that converts the value returned into a rate in degrees/second."
I found in datasheet "Full scale selection. Default value: 00
(00: 245 dps; 01: 500 dps; 1x: 2000 dps)"
So I understand, that when I divide default value to 245 than I get value in dps?

I would be grateful if you could help me

10

[quote]when I divide default value to 245 than I get value in dps?[/quote]No. That is the maximum possible reading, full scale rate of rotation.

You multiply each reading by the sensitivity value, which is given in the data sheet. For the 245 dps range setting, it is 0.00875 degrees/sec per digit.

11

Thank you, it helped :slight_smile:

Now I am trying to get angle possition on LCD. Here is my code:

[code]/*

  • main.c
  • Created on: 7 gru 2015
  •  Author: PC

*/

#include <avr/pgmspace.h>
#include <avr/eeprom.h>
#include <avr/io.h>
#include <avr/wdt.h>
#include <util/delay.h>
#include <avr/power.h>
#include "zyroskop.h"
#include "lcd.h"
#include <avr/interrupt.h>

float rate_x =0.0, rate_y =0.0,rate_z =0.0;
float off_x = 0.0, off_z = 0.0, off_y = 0.0;
int Gx1 = 0,Gx2 = 0,Gy1 = 0,Gy2 = 0,Gz1 = 0,Gz2 = 0;
float rate_x0 = 0.0;	// wartosci referencyjne
float fi0 = 0.0;		// wartosci referencyjne
float fi_x = 0.0;
float fi_y = 0.0;
float fi_z = 0.0;

int main()

{

LCD_Initalize();
LCD_Clear();

Inicjacja();
initgyro();

//pierwszy odczyt do offseta

_delay_ms(1000);
Gx1 = TWI_read_register(gyroscope_W ,OUT_X_L);
Gx2 = TWI_read_register(gyroscope_W ,OUT_X_H);

Gy1 = TWI_read_register(gyroscope_W ,OUT_Y_L);
Gy2 = TWI_read_register(gyroscope_W ,OUT_Y_H);

Gz1 = TWI_read_register(gyroscope_W ,OUT_Z_L);
Gz2 = TWI_read_register(gyroscope_W ,OUT_Z_H);


off_x=(Gx2 << 8)+Gx1;
off_y=(Gy2 << 8)+Gy1;
off_z=(Gz2 << 8)+Gz1;

TCCR0A |= (1<<WGM01);	// timer 8bitowy tryb CTC
TCCR0B |= (1<<CS01) | (1<<CS00); // preskeler na 64
OCR0A = 124;		// Dla czestotliwosci przerwan co 1000 Hz
TIMSK0 |= (1<<OCIE0A); //Compare Match A
sei();

////koniec

while(1)
 {
      Gx1 = TWI_read_register(gyroscope_W ,OUT_X_L);
      Gx2 = TWI_read_register(gyroscope_W ,OUT_X_H);

      Gy1 = TWI_read_register(gyroscope_W ,OUT_Y_L);
      Gy2 = TWI_read_register(gyroscope_W ,OUT_Y_H);

      Gz1 = TWI_read_register(gyroscope_W ,OUT_Z_L);
      Gz2 = TWI_read_register(gyroscope_W ,OUT_Z_H);

      //wartości w dps
      rate_x = ((Gx2 << 8)+Gx1-off_x)*7/800;
      rate_y = ((Gy2 << 8)+Gy1-off_y)*7/800;
      rate_z = ((Gz2 << 8)+Gz1-off_z)*7/800;


      ////////////////////////////////////

      // predkosc katowa => dfi/dt -> fi = calka(dps)dt

      ////////////////////////////////////

      	  	  	LCD_Clear();

       	   	   	LCD_GoTo(0,0);
                lcd_int((int)fi_x);

                LCD_GoTo(0,1);
                lcd_int((int)fi_z);

                LCD_GoTo(8,0);
                lcd_int((int)fi_y);

                _delay_ms(10);
 }

}

ISR (TIMER0_COMPA_vect) // Timer/Counter0 Compare Match A
{

							fi_x += rate_x/500;
							fi_y += rate_y/500;
							fi_z += rate_z/500;

}

[/code]
I sample with frequency 1000Hz. I don’t know why, but then I do integral it works better, when i divide to 500, not 1000
When I move Gyro slowly and not many times it shows good, but when I move it faster it loses its values. Am I doing anything wrong?

12

To do the integration you have to multiply each reading by dt, which is the actual elapsed time between readings, not the sample rate. An position/orientation update rate of 100 Hz is more than sufficient for most applications.

Measure the time using the millis() or micros() functions.