aboutsummaryrefslogtreecommitdiff
path: root/gyro.c
blob: 0333e863cb042f653f063fcc1f2b7ff302e24e31 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
#include "common.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/sleep.h>

#include "i2c.h"
#include "gyro.h"

/* device address */
#define L3GD20 0b11010100
/* registers */
#define L3GD20_WHOAMI 0xf
#define L3GD20_CTRLREG1 0x20
#define L3GD20_CTRLREG3 0x22
#define L3GD20_CTRLREG4 0x23
#define L3GD20_OUTZ 0x2c

/* raw z value */
static volatile int16_t zval = 0;
/* accumulated z value */
static int32_t zaccum = 0;
/* calculated zticks */
static int8_t zticks = 0;
/* currently reading from i2c */
static bool reading = false;

/* data ready interrupt
 */
ISR(PCINT0_vect) {
	/* empty */
}

void gyroInit () {
	/* set PB1 to input, with pull-up */
	DDRB = DDRB & ~((1 << DDB1));
	PORTB = PORTB | (1 << PORTB1);
	/* enable interrupt PCI0 */
	PCICR = PCICR | (1 << PCIE0);
	/* enable interrupts on PB1/PCINT1 */
	PCMSK0 = (1 << PCINT1);
}

/* XXX: make nonblocking */
void gyroStart () {
	/* configuration:
	 * disable power-down-mode, enable z
	 * defaults
	 * low-active (does not work?), push-pull, drdy on int2
	 * select 500dps
	 */
	uint8_t data[] = {0b00001100, 0b0, 0b00101000, 0b00010000};

	if (!twRequest (TWM_WRITE, L3GD20, L3GD20_CTRLREG1, data,
			sizeof (data)/sizeof (*data))) {
		printf ("cannot start write\n");
	}
	sleepwhile (twr.status == TWST_WAIT);
	printf ("final twi status was %i\n", twr.status);
}

/*	calculate ticks for z rotation
 */
static void gyroProcessTicks () {
	const uint8_t shift = 14;

	if (zaccum > (1 << shift)) {
		const uint32_t a = abs (zaccum);
		zticks += a >> shift;
		/* mask shift bits */
		zaccum -= a & (~0x3ff);
	} else if (zaccum < -(1 << shift)) {
		const uint32_t a = abs (zaccum);
		zticks -= a >> shift;
		zaccum += a & (~0x3ff);
	}
}

/*	process gyro sensor data, returns true if new data is available
 */
bool gyroProcess () {
	if (reading) {
		if (twr.status == TWST_OK) {
			/* new data transfered, process it */
			/* poor man's noise filter */
			if (abs (zval) > 64) {
				zaccum += zval;
			}
			gyroProcessTicks ();
			reading = false;
			return true;
		} else if (twr.status == TWST_ERR) {
			printf ("gyro i2c error\n");
			reading = false;
		}
	} else {
		if (((PINB >> PINB1) & 0x1) && twr.status != TWST_WAIT) {
			/* new data available in device buffer and bus is free */
			if (!twRequest (TWM_READ, L3GD20, L3GD20_OUTZ,
					(uint8_t *) &zval, sizeof (zval))) {
				printf ("cannot start read\n");
			} else {
				reading = true;
			}
		}
	}

	return false;
}

const int32_t gyroGetZAccum () {
	return zaccum;
}

void gyroResetZAccum () {
	zaccum = 0;
}

volatile const int16_t gyroGetZRaw () {
	return zval;
}

const int8_t gyroGetZTicks () {
	return zticks;
}