View source for How to interface ST L3GD20 gyroscope with the VisionSOM-6ULL

{{PageHeader|How to interface ST L3GD20 gyroscope with the VisionSOM-6ULL}}
== Description ==
[https://kamami.pl/kamod-kamami/557724-kamodl3gd20-modul-zyroskopu-mems-z-ukladem-l3gd20-firmy-stmicroelectronics.html KAmodL3GD20] is a gyroscope module with a L3GD20 chip from STMicroelectronics. It allows to measure angular velocity at ±2000 degrees per second. 

[[File:VisionCB_KAmodL3GD20_01.jpg|center|800px]]

== Prerequisites ==
This tutorial is based on a pre-prepared image for VisionSOM-6ULL module. The image is available here: [http://ftp.somlabs.com/VisionSOM-6ULL/SoMLabs-VisionSOM-6ULL-training-02-2018-sdcard-2gb.zip SoMLabs-VisionSOM-6ULL-training-02-2018-sdcard-2gb.zip]. 

Then you need to write the image file to microSD card. More information you can find in manuals: [[How to prepare SD Card with Debian 9.2 for VisionSOM-6ULL on Linux]] or [[How to prepare SD Card with Debian 9.2 for VisionSOM-6ULL on Windows]]. Please remember to use a proper image file: [http://ftp.somlabs.com/VisionSOM-6ULL/SoMLabs-VisionSOM-6ULL-training-02-2018-sdcard-2gb.zip SoMLabs-VisionSOM-6ULL-training-02-2018-sdcard-2gb.zip].


{| class="frame-green"
|-
| In the ready-to-use OS image, the kernel driver for the gyroscope has already been enabled in the kernel config, and the device has been added to the devicetree.
|}

If you need more information about how to customize, build and upload Device Tree file, please visit  [[How to customize Debian 9.2 device tree]].

== Hardware set-up ==
Connect the KAmodL3GD20 module according to the photo below:

[[File:VisionCB_KAmodL3GD20_02.JPG|center|800px]]

== Reading the values ==
To run the gyroscope, you need to go to the directory where the example code is located.
 root@localhost:~# cd /root/linux-academy/2-8

There is a program in C language ''gyro-i2c.c'' that calibrates and reads L3GD20 gyroscope:

<pre>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/i2c-dev.h>
#include <linux/i2c.h>
#include <sys/time.h>

/* L3GD20 internal registers */
#define WHO_AM_I        0x0F
#define CTRL_REG1       0x20
#define CTRL_REG2       0x21
#define CTRL_REG3       0x22
#define CTRL_REG4       0x23
#define CTRL_REG5       0x24
#define REFERENCE       0x25
#define OUT_TEMP        0x26
#define STATUS_REG      0x27
#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_REG   0x2E
#define FIFO_SRC_REG    0x2F
#define INT1_CFG        0x30
#define INT1_SRC        0x31
#define INT1_TSH_XH     0x32
#define INT1_TSH_XL     0x33
#define INT1_TSH_YH     0x34
#define INT1_TSH_YL     0x35
#define INT1_TSH_ZH     0x36
#define INT1_TSH_ZL     0x37
#define INT1_DURATION   0x38

#define GYRO_ADDR       0x6b
#define AUTO_INCREMENT  0x80

/* calibration values */
int x_low = 0, y_low = 0, z_low = 0;
int x_high = 0, y_high = 0, z_high = 0;


/*
 * get_timestamp
 */
static unsigned long
get_timestamp ()
{
  struct timeval tv;

  gettimeofday (&tv,NULL);
  return tv.tv_sec * 1000000UL + tv.tv_usec;
}

/*
 * gyro_init
 */
static int
gyro_init (int i2c_fd)
{
  unsigned char init_seq[6];

  init_seq[0] = (CTRL_REG1 | AUTO_INCREMENT);
  init_seq[1] = 0xCF; /* CTRL_REG1: normal mode, xyz enable */
  init_seq[2] = 0x01; /* CTRL_REG2: <default value> */
  init_seq[3] = 0x00; /* CTRL_REG3: <default value> */
  init_seq[4] = 0x80; /* CTRL_REG4: 250dps, Block Data Update */
  init_seq[5] = 0x02; /* CTRL_REG5: <default value> */

  if (write (i2c_fd, init_seq, 6) != 6)
    return -1;

  return 0;
}

/*
 * gyro_get_status
 */
static int
gyro_get_status (int i2c_fd)
{
  unsigned char reg_addr = STATUS_REG;
  unsigned char reg_data[1];
  int ret;

  struct i2c_msg messages[] =
    {
      {
        GYRO_ADDR,              /* slave address */
        0,                      /* flags: 0 */
        sizeof(reg_addr),       /* transfer size */
        &reg_addr               /* data */
      },

      {
        GYRO_ADDR,              /* slave adddress */
        I2C_M_RD,               /* flags: READ */
        sizeof(reg_data),       /* transfer size */
        reg_data                /* data */
      }
    };

  struct i2c_rdwr_ioctl_data packets =
    {
      messages,                                         /* address of messages */
      sizeof(messages) / sizeof(struct i2c_msg)         /* number of messages */
    };

  ret = ioctl (i2c_fd, I2C_RDWR, &packets);
  if (ret < 0)
    return ret;

  return (reg_data[0] & (1 << 3));
}

/*
 * gyro_get_xyz
 */
static int
gyro_get_xyz (int i2c_fd, float *x, float *y, float *z)
{
  unsigned char reg_addr = OUT_X_L | AUTO_INCREMENT;
  unsigned char reg_data[6];
  int ret;

  struct i2c_msg messages[] =
    {
      {
        GYRO_ADDR,              /* slave address */
        0,                      /* flags: 0 */
        sizeof(reg_addr),       /* transfer size */
        &reg_addr               /* data */
      },

      {
        GYRO_ADDR,              /* slave adddress */
        I2C_M_RD,               /* flags: READ */
        sizeof(reg_data),       /* transfer size */
        reg_data                /* data */
      }
    };

  struct i2c_rdwr_ioctl_data packets =
    {
      messages,                                         /* address of messages */
      sizeof(messages) / sizeof(struct i2c_msg)         /* number of messages */
    };

  ret = ioctl (i2c_fd, I2C_RDWR, &packets);
  if (ret < 0)
    return ret;

  *x = (short) (reg_data[0] + ((short)reg_data[1] << 8));
  *y = (short) (reg_data[2] + ((short)reg_data[3] << 8));
  *z = (short) (reg_data[4] + ((short)reg_data[5] << 8));

  return 0;
}

/*
 * gyro_calib
 */
static int
gyro_calib (int i2c_fd)
{
  float x_raw, y_raw, z_raw;
  int ret;

  for (int i =0 ; i < 200 ; i++)
    {
      /* check when a new set of data is available */
      while (!gyro_get_status (i2c_fd));

      /* read xyz raw values */
      ret = gyro_get_xyz (i2c_fd, &x_raw, &y_raw, &z_raw);
      if (ret < 0)
        break;

      /* thresholds for x-axis */
      if (x_raw > x_high)
        x_high = x_raw;
      else if (x_raw < x_low)
        x_low = x_raw;

      /* thresholds for y-axis */
      if (y_raw > y_high)
        y_high = y_raw;
      else if (y_raw < y_low)
        y_low = y_raw;

      /* thresholds for z-axis */
      if (z_raw > z_high)
        z_high = z_raw;
      else if (z_raw < z_low)
        z_low = z_raw;
    }

  return ret;
}

/*
 * main
 */
int
main (void)
{
  int i2c_fd, ret;
  float x_raw, y_raw, z_raw;
  unsigned long pt = 0;

  /* actual angles */
  float angX = 0;
  float angY = 0;
  float angZ = 0;

  /* previous angles for calculation */
  float p_angX = 0;
  float p_angY = 0;
  float p_angZ = 0;

  /* open i2c device */
  i2c_fd = open ("/dev/i2c-1", O_RDWR);
  if (i2c_fd < 0)
    {
      printf ("Failed to open the i2c bus\n");
      return EXIT_FAILURE;
    }

  /* set slave address */
  ret = ioctl (i2c_fd, I2C_SLAVE, GYRO_ADDR);
  if (ret < 0)
    {
      printf ("Failed to acquire bus access and/or talk to slave\n");
      goto exit;
    }

  /* gyro init */
  ret = gyro_init (i2c_fd);
  if (ret < 0)
    {
      printf ("gyro_init error!\n");
      goto exit;
    }

  /* gyro calib */
  puts ("Calibration...");
  ret = gyro_calib (i2c_fd);
  if (ret < 0)
    {
      printf ("gyro_calib error!\n");
      goto exit;
    }

  /* infinite loop */
  while (1)
    {
      /* check when a new set of data is available */
      while (!gyro_get_status (i2c_fd));

      /* read xyz raw values */
      gyro_get_xyz (i2c_fd, &x_raw, &y_raw, &z_raw);

      /* get timestamp */
      unsigned long ct = get_timestamp();
      if (pt == 0)
        {
          pt = get_timestamp();
          continue;
        }
      float dt = (float) (ct - pt) / 1000000.0;
      pt = get_timestamp();

      /* x-axis */
      if (x_raw >= x_high || x_raw <= x_low)
        {
          angX += ((p_angX + (x_raw * 0.00875))/2) * dt;
          p_angX = x_raw * 0.00875;
        }
      else
        p_angX = 0;

      /* y-axis */
      if (y_raw >= y_high || y_raw <= y_low)
        {
          angY += ((p_angY + (y_raw * 0.00875))/2) * dt;
          p_angY = y_raw * 0.00875;
        }
      else
        p_angY = 0;

      /* z-axis */
      if (z_raw >= z_high || z_raw <= z_low)
        {
          angZ += ((p_angZ + (z_raw * 0.00875))/2) * dt;
          p_angZ = z_raw * 0.00875;
        }
      else
        p_angZ = 0;

      printf ("%.1f %.1f %.1f\n", angX, angY, angZ);
      fflush (stdout);
    }

exit:
  return EXIT_FAILURE;
}
root@localhost:~/linux-academy/2-8# gh || z_raw <= z_low)
-bash: syntax error near unexpected token `)'
, angX, angY, angZ);
      fflush (stdout);
    }

exit:
  return EXIT_FAILURE;
}
</pre>

To compile the program just type:
 gcc gyro-i2c.c -o gyro-i2c

Now, it's time to run the application. The readings of three axis values from L3HGD20 chip will be displayed after executing:

 root@localhost:~/linux-academy/2-8# ./gyro-i2c
Any move of the sensor board will be related with the readings changes:


[[File:VisionCB_KAmodL3GD20_03.JPG|center|800px]]