source: azure_iot_hub_f767zi/trunk/app_iothub_client/pinkit/accelerometer.c@ 457

// https://github.com/ms-iotkithol-jp/IoTKitHoLV3/blob/master/PinKitIoTHubApp/PinKitIoTHubApp/PinKit/Accelerometer.cs

#include <kernel.h>
#include <t_syslog.h>
#include <target_syssvc.h>
#include "pinkit.h"
#include "kernel_cfg.h"

#define ADXL345_ADDR (0x1d << 1)
// ADXL345 registers
#define R_DEVID (0x00)
#define R_THRESH_TAP (0x1D)
#define R_OFSX (0x1E)
#define R_OFSY (0x1F)
#define R_OFSZ (0x20)
#define R_DUR (0x21)
#define R_Latent (0x22)
#define R_Window (0x23)
#define R_THRESH_INACT (0x25)
#define R_TIME_INACT (0x26)
#define R_ACT_INACT_CTL (0x27)
#define R_THRESH_FF (0x28)
#define R_TIME_FF (0x29)
#define R_TAP_AXES (0x2A)
#define R_ACT_TAP_STATUS (0x2B)
#define R_BW_RATE (0x2C)
#define R_POWER_CTL (0x2D)
#define R_INT_ENABLE (0x2E)
#define R_INT_MAP (0x2F)
#define R_INT_SOURCE (0x30)
#define R_DATA_FORMAT (0x31)
#define R_DATAX0 (0x32)
#define R_DATAX1 (0x33)
#define R_DATAY0 (0x34)
#define R_DATAY1 (0x35)
#define R_DATAZ0 (0x36)
#define R_DATAZ1 (0x37)
#define R_FIFO_CTL (0x38)
#define R_FIFO_STATUS (0x39)

I2C_Handle_t *hi2c;
#define GPIO_I2C_CS 13
static int timeout = 1000;
char xyz[6];

int RegRead(uint8_t reg)
{
        ER ret;
        int result = -1;
        uint8_t rdata[1];

        digitalWrite(GPIO_I2C_CS, 0);

        ret = i2c_memread(hi2c, ADXL345_ADDR, (uint16_t)reg, I2C_MEMADD_SIZE_8BIT, rdata, 1, timeout);
        if (ret != E_OK)
                goto exit;

        digitalWrite(GPIO_I2C_CS, 1);

        result = rdata[0];
exit:
#ifdef DEBUG_ADXL345
        printf("R[0x%2X]=0x%02X\n", reg, rdata[0]);
#endif
        return result;
}

int RegReads(uint8_t reg, char *data, int len)
{
        ER ret;
        int result = -1;

        digitalWrite(GPIO_I2C_CS, 0);

        ret = i2c_memread(hi2c, ADXL345_ADDR, (uint16_t)reg, I2C_MEMADD_SIZE_8BIT, (uint8_t *)data, len, timeout);
        if (ret != E_OK)
                goto exit;

        result = 0;
exit:
        digitalWrite(GPIO_I2C_CS, 1);

#ifdef DEBUG_ADXL345
        for (int i = 0; i < len; i++)
        {
                printf("R[0x%2X]=0x%02X\n", (reg + i), data[i]);
        }
#endif
        return result;
}

int RegWrite(uint8_t reg, uint8_t val)
{
        ER ret;
        int result = -1;
        uint8_t wdata[1];

        wdata[0] = val;

        digitalWrite(GPIO_I2C_CS, 0);

        ret = i2c_memwrite(hi2c, ADXL345_ADDR, (uint16_t)reg, I2C_MEMADD_SIZE_8BIT, wdata, 1, timeout);
        if (ret != E_OK)
                goto exit;

        digitalWrite(GPIO_I2C_CS, 1);

        result = 0;
exit:
        return result;
}

int RegWriteMask(uint8_t reg, uint8_t val, uint8_t mask)
{
        ER ret;
        int result = -1;
        uint8_t wdata[1];

        uint8_t tmp = RegRead(reg);

        wdata[0] = (uint8_t)(((int)tmp & ~(int)mask) | ((int)val & (int)mask));

        digitalWrite(GPIO_I2C_CS, 0);

        ret = i2c_memwrite(hi2c, ADXL345_ADDR, (uint16_t)reg, I2C_MEMADD_SIZE_8BIT, wdata, 1, timeout);
        if (ret != E_OK)
                goto exit;

        digitalWrite(GPIO_I2C_CS, 1);

        result = 0;
exit:
        return result;
}

uint8_t DeviceID()
{
        return RegRead(R_DEVID);
}

void ToSleep()
{
        RegWriteMask(R_POWER_CTL, 0x04, 0x04);
}

void ToWakeup()
{
        RegWriteMask(R_POWER_CTL, 0x00, 0x04);
}

// Normal Power
// Code     Output  BandWidth
// 0b1111   3200Hz  (1600Hz)
// 0b1110   1600Hz  (800Hz)
// 0b1101   800Hz   (40Hz)
// 0b1100   400Hz   (200Hz)
// 0b1011   200Hz   (100Hz)
// 0b1010   100Hz   (50Hz)
// 0b1001   50Hz    (25H)
// 0b1000   25Hz    (12.5Hz)
// 0b0111   12.5Hz  (6.25Hz)
// 0b0110   6.25Hz  (3.125Hz)
// Low Power
// 0b1100   400Hz   (200Hz)
// 0b1011   200Hz   (100Hz)
// 0b1010   100Hz   (50Hz)
// 0b1001   50Hz    (25H)
// 0b1000   25Hz    (12.5Hz)
// 0b0111   12.5Hz  (6.25Hz)
void SetBW_RATE(uint8_t n)
{
        RegWrite(R_BW_RATE, n);
}

// D7: SELF_TEST
// D6: SPI
// D5: INT_INVERT
// D4: 0
// D3: FULL_RES
// D2: Justfy
// D1-D0: Range
// 0 - 0: +-2g
// 0 - 1: +-4g
// 1 - 0: +-8g
// 1 - 1: +-16g
void SetDataFormat(uint8_t n)
{
        RegWrite(R_DATA_FORMAT, n);
}

void SetFullResolution()
{
        RegWriteMask(R_DATA_FORMAT, 0x08, 0x08);
}

void Measure()
{
        RegWriteMask(R_POWER_CTL, 0x08, 0x08);
}

static int ReadXYZ(int16_t *x, int16_t *y, int16_t *z)
{
        int result;
        result = RegReads(R_DATAX0, xyz, 6);
        *x = (int16_t)(((uint16_t)xyz[1] << 8) + (uint16_t)xyz[0]);
        *y = (int16_t)(((uint16_t)xyz[3] << 8) + (uint16_t)xyz[2]);
        *z = (int16_t)(((uint16_t)xyz[5] << 8) + (uint16_t)xyz[4]);
        return result;
}

static bool measuring = false;

/*
 *  GPIO設定関数
 */
static void
pinMode(uint8_t no, uint32_t mode)
{
        Arduino_PortControlBlock *pgcb = getGpioTable(DIGITAL_PIN, no);
        GPIO_Init_t GPIO_Init_Data;

        if(pgcb == NULL)
                return;
        pinClock(no);
        GPIO_Init_Data.mode  = mode;
        GPIO_Init_Data.pull  = GPIO_PULLUP;
        GPIO_Init_Data.otype = GPIO_OTYPE_PP;
        GPIO_Init_Data.speed = GPIO_SPEED_FAST;
        gpio_setup(pgcb->giobase, &GPIO_Init_Data, pgcb->giopin);
}

bool Accelerometer_Init(int clockRate, int tmout)
{
        I2C_Init_t i2c_initd;

        timeout = tmout;

        i2c_initd.Timing          = 0x00D00E28;
        i2c_initd.AddressingMode  = I2C_ADDRESSINGMODE_7BIT;
        i2c_initd.DualAddressMode = I2C_DUALADDRESS_DISABLE;
        i2c_initd.GeneralCallMode = I2C_GENERALCALL_DISABLE;
        i2c_initd.NoStretchMode   = I2C_NOSTRETCH_DISABLE;
        i2c_initd.OwnAddress1     = 0;
        i2c_initd.OwnAddress2     = 0;
        i2c_initd.semid           = I2CTRS_SEM;
        i2c_initd.semlock         = I2CLOC_SEM;

        if ((hi2c = i2c_init(I2C_PORTID, &i2c_initd)) == NULL) {
                /* Initialization Error */
                syslog_0(LOG_ERROR, "## I2C ERROR(1) ##");
                return false;
        }

        pinMode(GPIO_I2C_CS, GPIO_MODE_OUTPUT);

        ToWakeup();
        dly_tsk(10);                    // 10[ms]

        SetDataFormat(0x01);    // +-2g 10bit
        SetBW_RATE(0x19);       // sampling rate 50Hz

        return true;
}

bool Accelerometer_TakeMeasurements(SensorReading *result)
{
        if (result == NULL)
                return false;

        if (!measuring)
        {
                Measure();
        }

        int16_t x, y, z;
        int ret = ReadXYZ(&x, &y, &z);

        result->X = ((double)x) / 127.0;
        result->Y = ((double)y) / 127.0;
        result->Z = ((double)z) / 127.0;

        return ret == 0;
}