l3g4200d.c

#include "l3g4200d.h"
#include "spidrv.h"
#include "uart.h"
#include <stdio.h>

#define DEBUG 1
#define MOD_NAME "l3g4200d.c"

void l3g4200d_init()
{
  l3g4200d_initExti();
  l3g4200d_initInt();
  uint8_t did = spi2_read(SPI_GYRO, GYRO_REG_ID);
  if (did != GYRO_ID_VAL)
  {
#ifdef DEBUG
    //printf("%s %s - l3g4200d_init() - wrong device ID %i (expected %i) \n\r", ERR, MOD_NAME, did, GYRO_ID_VAL);
    uart_print("l3g4200d.c - l3g4200d_init - ERROR: WRONG DEVICE ID!!!\n\r");
#endif
    return;
  }
  // all these readings gives normal values
  uint8_t ctrl1 = spi2_read(SPI_GYRO, GYRO_REG_CTRL_1);
  uint8_t ctrl2 = spi2_read(SPI_GYRO, GYRO_REG_CTRL_2);
  uint8_t ctrl3 = spi2_read(SPI_GYRO, GYRO_REG_CTRL_3);
  uint8_t ctrl4 = spi2_read(SPI_GYRO, GYRO_REG_CTRL_4);
  uint8_t ctrl5 = spi2_read(SPI_GYRO, GYRO_REG_CTRL_5);

  spi2_write(SPI_GYRO, GYRO_REG_CTRL_2, GYRO_HPCOFF15);
  spi2_write(SPI_GYRO, GYRO_REG_CTRL_3, GYRO_DRDYINT);
  // if i'll try to read after writing i will getting 0xFF till gyroscope reset
  ctrl3 = spi2_read(SPI_GYRO, GYRO_REG_CTRL_3);
  spi2_write(SPI_GYRO, GYRO_REG_CTRL_4, GYRO_FS500 | GYRO_BDU );//| GYRO_BIGENDIAN);
  spi2_write(SPI_GYRO, GYRO_REG_CTRL_5, 0x00);
  spi2_write(SPI_GYRO, GYRO_REG_CTRL_1, GYRO_PWRON | GYRO_ODR400C25 | GYRO_ZYXEN);
#ifdef DEBUG
  //printf("%s %s - l3g4200d_init() - OK\n\r", DBG, MOD_NAME);
  uart_print("l3g4200d.c - l3g4200d_init - OK\n\r");
#endif
}