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
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
|
/*
* Operations for the l3g4200
*/
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <arch/board/board.h>
#include <nuttx/spi.h>
#include "sensors.h"
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
#define ADDR_INCREMENT (1<<6)
#define ADDR_WHO_AM_I 0x0f
#define WHO_I_AM 0xd4
#define ADDR_CTRL_REG1 0x20 /* sample rate constants are in the public header */
#define REG1_POWER_NORMAL (1<<3)
#define REG1_Z_ENABLE (1<<2)
#define REG1_Y_ENABLE (1<<1)
#define REG1_X_ENABLE (1<<0)
#define ADDR_CTRL_REG2 0x21
/* high-pass filter - usefulness TBD */
#define ADDR_CTRL_REG3 0x22
#define ADDR_CTRL_REG4 0x23
#define REG4_BDU (1<<7)
#define REG4_BIG_ENDIAN (1<<6)
#define REG4_SPI_3WIRE (1<<0)
#define ADDR_CTRL_REG5 0x24
#define REG5_BOOT (1<<7)
#define REG5_FIFO_EN (1<<6)
#define REG5_HIGHPASS_ENABLE (1<<4)
#define ADDR_REFERENCE 0x25
#define ADDR_TEMPERATURE 0x26
#define ADDR_STATUS_REG 0x27
#define STATUS_ZYXOR (1<<7)
#define SATAUS_ZOR (1<<6)
#define STATUS_YOR (1<<5)
#define STATUS_XOR (1<<4)
#define STATUS_ZYXDA (1<<3)
#define STATUS_ZDA (1<<2)
#define STATUS_YDA (1<<1)
#define STATUS_XDA (1<<0)
#define ADDR_OUT_X 0x28 /* 16 bits */
#define ADDR_OUT_Y 0x2A /* 16 bits */
#define ADDR_OUT_Z 0x2C /* 16 bits */
#define ADDR_FIFO_CTRL 0x2e
#define FIFO_MODE_BYPASS (0<<5)
#define FIFO_MODE_FIFO (1<<5)
#define FIFO_MODE_STREAM (2<<5)
#define FIFO_MODE_STREAM_TO_FIFO (3<<5)
#define FIFO_MODE_BYPASS_TO_STREAM (4<<5)
#define FIFO_THRESHOLD_MASK 0x1f
#define ADDR_FIFO_SRC 0x2f
#define FIFO_THREHSHOLD_OVER (1<<7)
#define FIFO_OVERRUN (1<<6)
#define FIFO_EMPTY (1<<5)
#define L3G4200_RATE_100Hz ((0<<6) | (0<<4))
#define L3G4200_RATE_200Hz ((1<<6) | (0<<4))
#define L3G4200_RATE_400Hz ((2<<6) | (1<<4))
#define L3G4200_RATE_800Hz ((3<<6) | (2<<4))
#define L3G4200_RANGE_250dps (0<<4)
#define L3G4200_RANGE_500dps (1<<4)
#define L3G4200_RANGE_2000dps (3<<4)
static void
write_reg(struct spi_dev_s *spi, uint8_t address, uint8_t data)
{
uint8_t cmd[2] = { address | DIR_WRITE, data };
SPI_SELECT(spi, PX4_SPIDEV_GYRO, true);
SPI_SNDBLOCK(spi, &cmd, sizeof(cmd));
SPI_SELECT(spi, PX4_SPIDEV_GYRO, false);
}
static uint8_t
read_reg(struct spi_dev_s *spi, uint8_t address)
{
uint8_t cmd[2] = {address | DIR_READ, 0};
uint8_t data[2];
SPI_SELECT(spi, PX4_SPIDEV_GYRO, true);
SPI_EXCHANGE(spi, cmd, data, sizeof(cmd));
SPI_SELECT(spi, PX4_SPIDEV_GYRO, false);
return data[1];
}
int
l3gd20_test_configure(struct spi_dev_s *spi)
{
uint8_t id;
id = read_reg(spi, ADDR_WHO_AM_I);
if (id == WHO_I_AM)
{
message("L3GD20 SUCCESS: 0x%02x\n", id);
}
else
{
message("L3GD20 FAIL: 0x%02x\n", id);
}
struct { /* status register and data as read back from the device */
uint8_t cmd;
uint8_t temp;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} __attribute__((packed)) report;
report.cmd = 0x26 | DIR_READ | ADDR_INCREMENT;
write_reg(spi, ADDR_CTRL_REG2, 0); /* disable high-pass filters */
write_reg(spi, ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */
write_reg(spi, ADDR_CTRL_REG5, 0); /* turn off FIFO mode */
write_reg(spi, ADDR_CTRL_REG4, ((3<<4) & 0x30) | REG4_BDU);
write_reg(spi, ADDR_CTRL_REG1,
(((2<<6) | (1<<4)) & 0xf0) | REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
SPI_SELECT(spi, PX4_SPIDEV_GYRO, true);
SPI_EXCHANGE(spi, &report, &report, sizeof(report));
SPI_SELECT(spi, PX4_SPIDEV_GYRO, false);
message("Init-read: gyro: x: %d\ty: %d\tz: %d\n", report.x, report.y, report.z);
usleep(1000);
//message("got id 0x%02x, expected ID 0xd4\n", id);
return 0;
}
int
l3gd20_test_read(struct spi_dev_s *spi)
{
struct { /* status register and data as read back from the device */
uint8_t cmd;
uint8_t temp;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} __attribute__((packed)) report;
report.cmd = 0x26 | DIR_READ | ADDR_INCREMENT;
SPI_LOCK(spi, true);
SPI_SELECT(spi, PX4_SPIDEV_GYRO, true);
SPI_EXCHANGE(spi, &report, &report, sizeof(report));
SPI_SELECT(spi, PX4_SPIDEV_GYRO, false);
SPI_LOCK(spi, false);
message("gyro: x: %d\ty: %d\tz: %d\n", report.x, report.y, report.z);
usleep(1000);
return 0;
}
|
