1 | // https://github.com/ms-iotkithol-jp/IoTKitHoLV3/blob/master/PinKitIoTHubApp/PinKitIoTHubApp/PinKit/Accelerometer.cs
|
---|
2 |
|
---|
3 | #include <kernel.h>
|
---|
4 | #include <t_syslog.h>
|
---|
5 | #include <target_syssvc.h>
|
---|
6 | #include "pinkit.h"
|
---|
7 | #include "kernel_cfg.h"
|
---|
8 |
|
---|
9 | #define ADXL345_ADDR (0x1d << 1)
|
---|
10 | // ADXL345 registers
|
---|
11 | #define R_DEVID (0x00)
|
---|
12 | #define R_THRESH_TAP (0x1D)
|
---|
13 | #define R_OFSX (0x1E)
|
---|
14 | #define R_OFSY (0x1F)
|
---|
15 | #define R_OFSZ (0x20)
|
---|
16 | #define R_DUR (0x21)
|
---|
17 | #define R_Latent (0x22)
|
---|
18 | #define R_Window (0x23)
|
---|
19 | #define R_THRESH_INACT (0x25)
|
---|
20 | #define R_TIME_INACT (0x26)
|
---|
21 | #define R_ACT_INACT_CTL (0x27)
|
---|
22 | #define R_THRESH_FF (0x28)
|
---|
23 | #define R_TIME_FF (0x29)
|
---|
24 | #define R_TAP_AXES (0x2A)
|
---|
25 | #define R_ACT_TAP_STATUS (0x2B)
|
---|
26 | #define R_BW_RATE (0x2C)
|
---|
27 | #define R_POWER_CTL (0x2D)
|
---|
28 | #define R_INT_ENABLE (0x2E)
|
---|
29 | #define R_INT_MAP (0x2F)
|
---|
30 | #define R_INT_SOURCE (0x30)
|
---|
31 | #define R_DATA_FORMAT (0x31)
|
---|
32 | #define R_DATAX0 (0x32)
|
---|
33 | #define R_DATAX1 (0x33)
|
---|
34 | #define R_DATAY0 (0x34)
|
---|
35 | #define R_DATAY1 (0x35)
|
---|
36 | #define R_DATAZ0 (0x36)
|
---|
37 | #define R_DATAZ1 (0x37)
|
---|
38 | #define R_FIFO_CTL (0x38)
|
---|
39 | #define R_FIFO_STATUS (0x39)
|
---|
40 |
|
---|
41 | I2C_Handle_t *hi2c;
|
---|
42 | #define GPIO_I2C_CS 13
|
---|
43 | static int timeout = 1000;
|
---|
44 | char xyz[6];
|
---|
45 |
|
---|
46 | int RegRead(uint8_t reg)
|
---|
47 | {
|
---|
48 | ER ret;
|
---|
49 | int result = -1;
|
---|
50 | uint8_t rdata[1];
|
---|
51 |
|
---|
52 | digitalWrite(GPIO_I2C_CS, 0);
|
---|
53 |
|
---|
54 | ret = i2c_memread(hi2c, ADXL345_ADDR, (uint16_t)reg, I2C_MEMADD_SIZE_8BIT, rdata, 1, timeout);
|
---|
55 | if (ret != E_OK)
|
---|
56 | goto exit;
|
---|
57 |
|
---|
58 | digitalWrite(GPIO_I2C_CS, 1);
|
---|
59 |
|
---|
60 | result = rdata[0];
|
---|
61 | exit:
|
---|
62 | #ifdef DEBUG_ADXL345
|
---|
63 | printf("R[0x%2X]=0x%02X\n", reg, rdata[0]);
|
---|
64 | #endif
|
---|
65 | return result;
|
---|
66 | }
|
---|
67 |
|
---|
68 | int RegReads(uint8_t reg, char *data, int len)
|
---|
69 | {
|
---|
70 | ER ret;
|
---|
71 | int result = -1;
|
---|
72 |
|
---|
73 | digitalWrite(GPIO_I2C_CS, 0);
|
---|
74 |
|
---|
75 | ret = i2c_memread(hi2c, ADXL345_ADDR, (uint16_t)reg, I2C_MEMADD_SIZE_8BIT, (uint8_t *)data, len, timeout);
|
---|
76 | if (ret != E_OK)
|
---|
77 | goto exit;
|
---|
78 |
|
---|
79 | result = 0;
|
---|
80 | exit:
|
---|
81 | digitalWrite(GPIO_I2C_CS, 1);
|
---|
82 |
|
---|
83 | #ifdef DEBUG_ADXL345
|
---|
84 | for (int i = 0; i < len; i++)
|
---|
85 | {
|
---|
86 | printf("R[0x%2X]=0x%02X\n", (reg + i), data[i]);
|
---|
87 | }
|
---|
88 | #endif
|
---|
89 | return result;
|
---|
90 | }
|
---|
91 |
|
---|
92 | int RegWrite(uint8_t reg, uint8_t val)
|
---|
93 | {
|
---|
94 | ER ret;
|
---|
95 | int result = -1;
|
---|
96 | uint8_t wdata[1];
|
---|
97 |
|
---|
98 | wdata[0] = val;
|
---|
99 |
|
---|
100 | digitalWrite(GPIO_I2C_CS, 0);
|
---|
101 |
|
---|
102 | ret = i2c_memwrite(hi2c, ADXL345_ADDR, (uint16_t)reg, I2C_MEMADD_SIZE_8BIT, wdata, 1, timeout);
|
---|
103 | if (ret != E_OK)
|
---|
104 | goto exit;
|
---|
105 |
|
---|
106 | digitalWrite(GPIO_I2C_CS, 1);
|
---|
107 |
|
---|
108 | result = 0;
|
---|
109 | exit:
|
---|
110 | return result;
|
---|
111 | }
|
---|
112 |
|
---|
113 | int RegWriteMask(uint8_t reg, uint8_t val, uint8_t mask)
|
---|
114 | {
|
---|
115 | ER ret;
|
---|
116 | int result = -1;
|
---|
117 | uint8_t wdata[1];
|
---|
118 |
|
---|
119 | uint8_t tmp = RegRead(reg);
|
---|
120 |
|
---|
121 | wdata[0] = (uint8_t)(((int)tmp & ~(int)mask) | ((int)val & (int)mask));
|
---|
122 |
|
---|
123 | digitalWrite(GPIO_I2C_CS, 0);
|
---|
124 |
|
---|
125 | ret = i2c_memwrite(hi2c, ADXL345_ADDR, (uint16_t)reg, I2C_MEMADD_SIZE_8BIT, wdata, 1, timeout);
|
---|
126 | if (ret != E_OK)
|
---|
127 | goto exit;
|
---|
128 |
|
---|
129 | digitalWrite(GPIO_I2C_CS, 1);
|
---|
130 |
|
---|
131 | result = 0;
|
---|
132 | exit:
|
---|
133 | return result;
|
---|
134 | }
|
---|
135 |
|
---|
136 | uint8_t DeviceID()
|
---|
137 | {
|
---|
138 | return RegRead(R_DEVID);
|
---|
139 | }
|
---|
140 |
|
---|
141 | void ToSleep()
|
---|
142 | {
|
---|
143 | RegWriteMask(R_POWER_CTL, 0x04, 0x04);
|
---|
144 | }
|
---|
145 |
|
---|
146 | void ToWakeup()
|
---|
147 | {
|
---|
148 | RegWriteMask(R_POWER_CTL, 0x00, 0x04);
|
---|
149 | }
|
---|
150 |
|
---|
151 | // Normal Power
|
---|
152 | // Code Output BandWidth
|
---|
153 | // 0b1111 3200Hz (1600Hz)
|
---|
154 | // 0b1110 1600Hz (800Hz)
|
---|
155 | // 0b1101 800Hz (40Hz)
|
---|
156 | // 0b1100 400Hz (200Hz)
|
---|
157 | // 0b1011 200Hz (100Hz)
|
---|
158 | // 0b1010 100Hz (50Hz)
|
---|
159 | // 0b1001 50Hz (25H)
|
---|
160 | // 0b1000 25Hz (12.5Hz)
|
---|
161 | // 0b0111 12.5Hz (6.25Hz)
|
---|
162 | // 0b0110 6.25Hz (3.125Hz)
|
---|
163 | // Low Power
|
---|
164 | // 0b1100 400Hz (200Hz)
|
---|
165 | // 0b1011 200Hz (100Hz)
|
---|
166 | // 0b1010 100Hz (50Hz)
|
---|
167 | // 0b1001 50Hz (25H)
|
---|
168 | // 0b1000 25Hz (12.5Hz)
|
---|
169 | // 0b0111 12.5Hz (6.25Hz)
|
---|
170 | void SetBW_RATE(uint8_t n)
|
---|
171 | {
|
---|
172 | RegWrite(R_BW_RATE, n);
|
---|
173 | }
|
---|
174 |
|
---|
175 | // D7: SELF_TEST
|
---|
176 | // D6: SPI
|
---|
177 | // D5: INT_INVERT
|
---|
178 | // D4: 0
|
---|
179 | // D3: FULL_RES
|
---|
180 | // D2: Justfy
|
---|
181 | // D1-D0: Range
|
---|
182 | // 0 - 0: +-2g
|
---|
183 | // 0 - 1: +-4g
|
---|
184 | // 1 - 0: +-8g
|
---|
185 | // 1 - 1: +-16g
|
---|
186 | void SetDataFormat(uint8_t n)
|
---|
187 | {
|
---|
188 | RegWrite(R_DATA_FORMAT, n);
|
---|
189 | }
|
---|
190 |
|
---|
191 | void SetFullResolution()
|
---|
192 | {
|
---|
193 | RegWriteMask(R_DATA_FORMAT, 0x08, 0x08);
|
---|
194 | }
|
---|
195 |
|
---|
196 | void Measure()
|
---|
197 | {
|
---|
198 | RegWriteMask(R_POWER_CTL, 0x08, 0x08);
|
---|
199 | }
|
---|
200 |
|
---|
201 | static int ReadXYZ(int16_t *x, int16_t *y, int16_t *z)
|
---|
202 | {
|
---|
203 | int result;
|
---|
204 | result = RegReads(R_DATAX0, xyz, 6);
|
---|
205 | *x = (int16_t)(((uint16_t)xyz[1] << 8) + (uint16_t)xyz[0]);
|
---|
206 | *y = (int16_t)(((uint16_t)xyz[3] << 8) + (uint16_t)xyz[2]);
|
---|
207 | *z = (int16_t)(((uint16_t)xyz[5] << 8) + (uint16_t)xyz[4]);
|
---|
208 | return result;
|
---|
209 | }
|
---|
210 |
|
---|
211 | static bool measuring = false;
|
---|
212 |
|
---|
213 | /*
|
---|
214 | * GPIO設定関数
|
---|
215 | */
|
---|
216 | static void
|
---|
217 | pinMode(uint8_t no, uint32_t mode)
|
---|
218 | {
|
---|
219 | Arduino_PortControlBlock *pgcb = getGpioTable(DIGITAL_PIN, no);
|
---|
220 | GPIO_Init_t GPIO_Init_Data;
|
---|
221 |
|
---|
222 | if(pgcb == NULL)
|
---|
223 | return;
|
---|
224 | pinClock(no);
|
---|
225 | GPIO_Init_Data.mode = mode;
|
---|
226 | GPIO_Init_Data.pull = GPIO_PULLUP;
|
---|
227 | GPIO_Init_Data.otype = GPIO_OTYPE_PP;
|
---|
228 | GPIO_Init_Data.speed = GPIO_SPEED_FAST;
|
---|
229 | gpio_setup(pgcb->giobase, &GPIO_Init_Data, pgcb->giopin);
|
---|
230 | }
|
---|
231 |
|
---|
232 | bool Accelerometer_Init(int clockRate, int tmout)
|
---|
233 | {
|
---|
234 | I2C_Init_t i2c_initd;
|
---|
235 |
|
---|
236 | timeout = tmout;
|
---|
237 |
|
---|
238 | i2c_initd.Timing = 0x00D00E28;
|
---|
239 | i2c_initd.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
|
---|
240 | i2c_initd.DualAddressMode = I2C_DUALADDRESS_DISABLE;
|
---|
241 | i2c_initd.GeneralCallMode = I2C_GENERALCALL_DISABLE;
|
---|
242 | i2c_initd.NoStretchMode = I2C_NOSTRETCH_DISABLE;
|
---|
243 | i2c_initd.OwnAddress1 = 0;
|
---|
244 | i2c_initd.OwnAddress2 = 0;
|
---|
245 | i2c_initd.semid = I2CTRS_SEM;
|
---|
246 | i2c_initd.semlock = I2CLOC_SEM;
|
---|
247 |
|
---|
248 | if ((hi2c = i2c_init(I2C_PORTID, &i2c_initd)) == NULL) {
|
---|
249 | /* Initialization Error */
|
---|
250 | syslog_0(LOG_ERROR, "## I2C ERROR(1) ##");
|
---|
251 | return false;
|
---|
252 | }
|
---|
253 |
|
---|
254 | pinMode(GPIO_I2C_CS, GPIO_MODE_OUTPUT);
|
---|
255 |
|
---|
256 | ToWakeup();
|
---|
257 | dly_tsk(10); // 10[ms]
|
---|
258 |
|
---|
259 | SetDataFormat(0x01); // +-2g 10bit
|
---|
260 | SetBW_RATE(0x19); // sampling rate 50Hz
|
---|
261 |
|
---|
262 | return true;
|
---|
263 | }
|
---|
264 |
|
---|
265 | bool Accelerometer_TakeMeasurements(SensorReading *result)
|
---|
266 | {
|
---|
267 | if (result == NULL)
|
---|
268 | return false;
|
---|
269 |
|
---|
270 | if (!measuring)
|
---|
271 | {
|
---|
272 | Measure();
|
---|
273 | }
|
---|
274 |
|
---|
275 | int16_t x, y, z;
|
---|
276 | int ret = ReadXYZ(&x, &y, &z);
|
---|
277 |
|
---|
278 | result->X = ((double)x) / 127.0;
|
---|
279 | result->Y = ((double)y) / 127.0;
|
---|
280 | result->Z = ((double)z) / 127.0;
|
---|
281 |
|
---|
282 | return ret == 0;
|
---|
283 | }
|
---|