Gyroscope Driver

Configuring and reading an L3GD20. More...

Files
file	gyro.h
	L3GD20 Gyroscope API Header.
file	gyro.c
	L3GD20 Gyroscope API Implementation.

Macros
#define	GYROREG_CTRL1   0x20
	Gyroscope Control Register 1. More...
#define	GYROREG_CTRL4   0x23
	Gyroscope Control Register 4. More...
#define	GYROREG_OUTXL   0x28
	First Gyroscope Output Register. More...

Enumerations
enum	GyroRange { r250DPS, r500DPS, r2000DPS }
	Gyroscope Range options. More...

Functions
Error	gyroInit (GyroRange r)
	Initializes the Gyroscope. More...
Error	gyroRead (Vector3f *v)
	Get a set of gyroscope data. More...
Error	gyroWriteByte (uint8_t reg, uint8_t val)
	Write a Gyroscope Register. More...

Variables
GyroRange	gyroRange
	Stored range to scale returned values. More...

Detailed Description

Configuring and reading an L3GD20.

Macro Definition Documentation

#define GYROREG_CTRL1   0x20

Gyroscope Control Register 1.

Definition at line 48 of file gyro.c.

Referenced by gyroInit().

#define GYROREG_CTRL4   0x23

Gyroscope Control Register 4.

Definition at line 49 of file gyro.c.

Referenced by gyroInit().

#define GYROREG_OUTXL   0x28

First Gyroscope Output Register.

Definition at line 50 of file gyro.c.

Referenced by gyroRead().

Enumeration Type Documentation

enum GyroRange

Gyroscope Range options.

Enumerator
r250DPS	+- 250 Degrees per Second
r500DPS	+- 500 Degrees per Second
r2000DPS	+- 2000 Degrees per Second

Definition at line 47 of file gyro.h.

             {
    r250DPS, 
    r500DPS, 
    r2000DPS, 
} GyroRange;

Function Documentation

Error gyroInit

(

GyroRange

r

)

Initializes the Gyroscope.

I2C should already be initialized.

Parameters

r	GyroRange to use

Returns

TWI_NO_ANSWER, TWI_WRITE_ERROR, ARGUMENT_ERROR or SUCCESS

Definition at line 75 of file gyro.c.

References ARGUMENT_ERROR, gyroRange, GYROREG_CTRL1, GYROREG_CTRL4, gyroWriteByte(), r2000DPS, r250DPS, r500DPS, and SUCCESS.

Referenced by orientationInit().

                            {
    uint8_t v;
    switch (r) {
        case r250DPS:
            v = 0x00;
            break;
        case r500DPS:
            v = 0x10;
            break;
        case r2000DPS:
            v = 0x20;
            break;
        default:
            return ARGUMENT_ERROR;
    }
    gyroRange = r;
    Error e = gyroWriteByte(GYROREG_CTRL1, 0x0F);
    if (e != SUCCESS) {
        return e;
    }
    e = gyroWriteByte(GYROREG_CTRL4, v);
    return e;
}

Error gyroRead

(

Vector3f *

v

)

Get a set of gyroscope data.

gyroInit() should already be called.

Parameters

v	Data Destionation

Returns

TWI_NO_ANSWER, TWI_WRITE_ERROR, ARGUMENT_ERROR or SUCCESS

Examples:

hardwareTest.c, and uartFlight.c.

Definition at line 99 of file gyro.c.

References ARGUMENT_ERROR, GYRO_ADDRESS, GYROFILTERFACTOR, gyroRange, GYROREG_OUTXL, r2000DPS, r250DPS, r500DPS, SUCCESS, TWI_NO_ANSWER, TWI_READ, TWI_WRITE, TWI_WRITE_ERROR, twiReadAck(), twiReadNak(), twiRepStart(), twiStart(), twiWrite(), Vector3f::x, Vector3f::y, and Vector3f::z.

Referenced by orientationTask().

                            {
    // Simple Software Low-Pass
    static double gyroSumX = 0, gyroSumY = 0, gyroSumZ = 0;
    static double gyroFilterX = 0, gyroFilterY = 0, gyroFilterZ = 0;

    if (v == NULL) {
        return ARGUMENT_ERROR;
    }
    if (twiStart(GYRO_ADDRESS | TWI_WRITE)) {
        return TWI_NO_ANSWER;
    }
    if (twiWrite(GYROREG_OUTXL | 0x80)) { // Auto Increment
        return TWI_WRITE_ERROR;
    }
    if (twiRepStart(GYRO_ADDRESS | TWI_READ)) {
        return TWI_NO_ANSWER;
    }

    uint8_t xl = twiReadAck();
    uint8_t xh = twiReadAck();
    uint8_t yl = twiReadAck();
    uint8_t yh = twiReadAck();
    uint8_t zl = twiReadAck();
    uint8_t zh = twiReadNak();

    int16_t x = *(int8_t *)(&xh);
    x *= (1 << 8);
    x |= xl;

    int16_t y = *(int8_t *)(&yh);
    y *= (1 << 8);
    y |= yl;

    int16_t z = *(int8_t *)(&zh);
    z *= (1 << 8);
    z |= zl;

    switch (gyroRange) {
        case r250DPS:
            v->x = (((double)x) * 250 / 0x8000);
            v->y = (((double)y) * 250 / 0x8000);
            v->z = (((double)z) * 250 / 0x8000);
            break;
        case r500DPS:
            v->x = (((double)x) * 500 / 0x8000);
            v->y = (((double)y) * 500 / 0x8000);
            v->z = (((double)z) * 500 / 0x8000);
            break;
        case r2000DPS:
            v->x = (((double)x) * 2000 / 0x8000);
            v->y = (((double)y) * 2000 / 0x8000);
            v->z = (((double)z) * 2000 / 0x8000);
            break;
        default:
            return ARGUMENT_ERROR;
    }

    gyroSumX = gyroSumX - gyroFilterX + v->x;
    gyroFilterX = gyroSumX / GYROFILTERFACTOR;
    v->x = gyroFilterX;

    gyroSumY = gyroSumY - gyroFilterY + v->y;
    gyroFilterY = gyroSumY / GYROFILTERFACTOR;
    v->y = gyroFilterY;

    gyroSumZ = gyroSumZ - gyroFilterZ + v->z;
    gyroFilterZ = gyroSumZ / GYROFILTERFACTOR;
    v->z = gyroFilterZ;

    return SUCCESS;
}

Error gyroWriteByte	(	uint8_t	reg,
		uint8_t	val
	)

Write a Gyroscope Register.

I2C should aready be initialized!

Parameters

reg	Register Address
val	New Value

Returns

TWI_NO_ANSWER, TWI_WRITE_ERROR or SUCCESS.

Definition at line 61 of file gyro.c.

References TWI_NO_ANSWER.

Referenced by gyroInit().

                                              {
    if (twiStart(GYRO_ADDRESS | TWI_WRITE)) {
        return TWI_NO_ANSWER;
    }
    if (twiWrite(reg)) {
        return TWI_WRITE_ERROR;
    }
    if (twiWrite(val)) {
        return TWI_WRITE_ERROR;
    }
    twiStop();
    return SUCCESS;
}

Variable Documentation

GyroRange gyroRange

Stored range to scale returned values.

Definition at line 52 of file gyro.c.

Referenced by gyroInit(), and gyroRead().