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
130
131
132
133
134
135
|
#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 int16_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))) {
puts ("cannot start write");
}
sleepwhile (twr.status == TWST_WAIT);
puts ("gyroStart done");
}
/* calculate ticks for z rotation
*/
static void gyroProcessTicks () {
const uint8_t shift = 14;
const uint32_t max = (1 << shift);
const uint32_t mask = ~(max-1);
if (zaccum > (int32_t) max) {
const uint32_t a = abs (zaccum);
zticks += a >> shift;
/* mask shift bits */
zaccum -= a & mask;
} else if (zaccum < -((int32_t) max)) {
const uint32_t a = abs (zaccum);
zticks -= a >> shift;
zaccum += a & mask;
}
}
/* 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) {
puts ("gyro i2c error");
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))) {
puts ("cannot start read");
} else {
reading = true;
}
}
}
return false;
}
int32_t gyroGetZAccum () {
return zaccum;
}
void gyroResetZAccum () {
zaccum = 0;
}
int16_t gyroGetZRaw () {
return zval;
}
int16_t gyroGetZTicks () {
return zticks;
}
void gyroResetZTicks () {
zticks = 0;
}
|
