[457] | 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 | }
|
---|