Personal tools

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

From SomLabs Wiki

Revision as of 21:58, 26 July 2018 by Pawelzbysinski (talk | contribs) (Reading the values)
(diff) ← Older revision | Latest revision (diff) | Newer revision → (diff)
Jump to: navigation, search

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


Description

KAmodL3GD20 is a gyroscope module with a L3GD20 chip from STMicroelectronics. It allows to measure angular velocity at ±2000 degrees per second.

VisionCB KAmodL3GD20 01.jpg

Prerequisites

This tutorial is based on a pre-prepared image for VisionSOM-6ULL module. The image is available here: 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: SoMLabs-VisionSOM-6ULL-training-02-2018-sdcard-2gb.zip.


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:

VisionCB KAmodL3GD20 02.JPG

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:

#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;
}

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:


VisionCB KAmodL3GD20 03.JPG
NXP Partner ST Partner Renesas Partner