gyroscope.c

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////
/// \file heivs/gyroscope.c
/// \brief ARMEBS4 gyroscope access functions
/// \author Pascal Sartoretti (sap at hevs dot ch)
//////////////////////////////////////////////////////////////////////////////////
#include "heivs/gyroscope.h"
#include "heivs/stm32_i2c.h"

// address of gyroscope L3GD20 mounted on ARMEBS4
#define I2C_DEV_ADDR        0x6A

#define GYR_WHO_AM_I        0x0F    // who am i, answer 0xD4
#define GYR_CTRL_REG1       0x20    // axis enable, power down, bandwidth
#define GYR_CTRL_REG2       0x21    // highpass filters
#define GYR_CTRL_REG3       0x22    // interrupts control
#define GYR_CTRL_REG4       0x23    // scale and data selection
#define GYR_CTRL_REG5       0x24    // miscanellous
#define GYR_REFERENCE       0x25    // reference register
#define GYR_OUT_TEMP        0x26    // temperature measure
#define GYR_STATUS_REG      0x27    // data status
#define GYR_OUT_X_L         0x28    // X value axis LSB
#define GYR_OUT_X_H         0x29    // X value axis MSB
#define GYR_OUT_Y_L         0x2A    // Y value axis LSB
#define GYR_OUT_Y_H         0x2B    // Y value axis MSB
#define GYR_OUT_Z_L         0x2C    // Z value axis LSB
#define GYR_OUT_Z_H         0x2D    // Z value axis MSB
#define GYR_FIFO_CTRL_REG   0x2E    // fifo control register
#define GYR_FIFO_SRC_REG    0x2F    // fifo status register
#define GYR_INT1_CFG        0x30    // interrupt control register
#define GYR_INT1_SRC        0x31    // interrupt status register
#define GYR_INT_TSH_XH      0x32    // X value treshold interrupt MSB
#define GYR_INT_TSH_XL      0x33    // X value treshold interrupt LSB
#define GYR_INT_TSH_YH      0x34    // Y value treshold interrupt MSB
#define GYR_INT_TSH_YL      0x35    // Y value treshold interrupt LSB
#define GYR_INT_TSH_ZH      0x36    // Z value treshold interrupt MSB
#define GYR_INT_TSH_ZL      0x37    // Z value treshold interrupt LSB
#define GYR_INT1_DURATION   0x38    // duration before interrupt

#define GYR_WHO_AM_I_DATA   0xD4    // signature of gyroscope

static const struct heivs_bus_t *bus = &bus_i2c[0];

//////////////////////////////////////////////////////////////////////////////////
/// \brief Read a data byte from gyroscope
/// \param address register address
/// \param data Pointer to data
/// \\return NO_ERROR for no problem
//////////////////////////////////////////////////////////////////////////////////
static status_e Gyro_RD(uint8_t address, uint8_t * data)
{
    status_e status;
    uint8_t regAddress;

    regAddress = address;
    status = bus_get(bus);
    if(status != NO_ERROR) return status;
    status = bus_writeread(bus,I2C_DEV_ADDR,&regAddress,1,data,1,NULL);
    if(status != NO_ERROR) return status;
    status = bus_release(bus);
    if(status != NO_ERROR) return status;

    return NO_ERROR;
}

status_e Gyro_Init(void)
{
    uint8_t data;
    status_e status;

    status = bus_init(bus);
    if(status != NO_ERROR)
        return status;

    status = Gyro_RD(GYR_WHO_AM_I,&data);
    if(status != NO_ERROR)
        return status;
    if(data != GYR_WHO_AM_I_DATA)
    {
        return ERROR_HW_FAILED; // bad gyro signature
    }
    status = Gyro_Write(GYR_CTRL_REG1, 0x0F);
    if(status != NO_ERROR)
        return status;

    return NO_ERROR;
}

status_e Gyro_Read(struct gyroscope_data_t * measure)
{
    status_e status;
    uint8_t regAddress;
    uint8_t data[6];

    regAddress = GYR_OUT_X_L | (1 << 7);    // multiple bytes read
    status = bus_get(bus);
    if(status != NO_ERROR) return status;
    status = bus_writeread(bus,I2C_DEV_ADDR,&regAddress,1,data,6,NULL);
    if(status != NO_ERROR) return status;
    status = bus_release(bus);
    if(status != NO_ERROR) return status;

    /**
     * To match the accelerometer axis as reference
     * the x and y channels must be swapped and y inverted.
     */
    measure->y = -(data[0] | (data[1] << 8));
    measure->x = +(data[2] | (data[3] << 8));
    measure->z = +(data[4] | (data[5] << 8));

    return NO_ERROR;
}

status_e Gyro_Write(uint8_t reg, uint8_t data )
{
    status_e status;
    uint8_t dta[2];

    dta[0] = reg;
    dta[1] = data;

    status = bus_get(bus);
    if(status != NO_ERROR) return status;
    status = bus_write(bus,I2C_DEV_ADDR,&dta,2);
    if(status != NO_ERROR) return status;
    status = bus_release(bus);
    if(status != NO_ERROR) return status;

    return NO_ERROR;
}