nanohub: drivers/st_acc44: Add support for STMicroelectronics ACC44 sensor driver
ST ACC44 is a MEMS-based accelerometer sensor. The current solution does not make
use of the sensor internal FIFO and works for I2C communication protocol only.
Signed-off-by: Armando Visconti <armando.visconti@st.com>
Change-Id: I01baaffbbdec7ad4006a8f64fd26ad249e770c9d
diff --git a/firmware/os/drivers/st_acc44/README b/firmware/os/drivers/st_acc44/README
new file mode 100644
index 0000000..7c7f4d5
--- /dev/null
+++ b/firmware/os/drivers/st_acc44/README
@@ -0,0 +1,54 @@
+STMicroelectronics acc44 accelerometer sensor device driver for Google nanohub.
+The driver uses the device in high-resolution mode with FS=8g.
+
+This drivers support following devices:
+ - LIS2DW12
+
+- Supported features:
+
+A. Reports accelerometer data
+B. Different data rates:
+C. I2C protocol
+D. Data ready reported by interrupt
+
+
+- Platform/variant porting:
+
+The driver requires that following macros are defined in the variant.h
+file of the specific variant:
+
+ ST_ACC44_I2C_BUS_ID /* specify I2C Bus ID */
+ ST_ACC44_I2C_SPEED /* specify I2C Bus speed in hz */
+ ST_ACC44_I2C_ADDR /* specify device I2C address */
+
+ ST_ACC44_INT_PIN /* specify the gpio used for the DRDY irq */
+ ST_ACC44_INT_IRQ /* specify the exti interrupt of ST_ACC44_INT_PIN */
+
+ ST_ACC44_TO_ANDROID_COORDINATE(x, y, z)
+ /* specify how axis has to be rotated according to variant platform
+ * orientation.
+ */
+
+Example:
+
+ /*
+ * Define platform/variant dependent ST_ACC44 device macros
+ */
+ #define ST_ACC44_DBG_ENABLED 1
+
+ /* I2C defs to be used when device is plugged to I2C bus */
+ #define ST_ACC44_I2C_BUS_ID 0
+ #define ST_ACC44_I2C_SPEED 400000
+ #define ST_ACC44_I2C_ADDR 0x19
+
+ #define ST_ACC44_INT_PIN GPIO_PC(5)
+ #define ST_ACC44_INT_IRQ EXTI9_5_IRQn
+
+#define ST_ACC44_TO_ANDROID_COORDINATE(x, y, z) \
+ do { \
+ float xi = x, yi = y, zi = z; \
+ x = xi; y = yi; z = zi; \
+ } while (0)
+
+If these macros are not defined in the current variant the driver forces a compilation
+error.
diff --git a/firmware/os/drivers/st_acc44/st_acc44.c b/firmware/os/drivers/st_acc44/st_acc44.c
new file mode 100644
index 0000000..e6abca5
--- /dev/null
+++ b/firmware/os/drivers/st_acc44/st_acc44.c
@@ -0,0 +1,843 @@
+/*
+ * Copyright (C) 2017 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <atomic.h>
+#include <gpio.h>
+#include <isr.h>
+#include <nanohubPacket.h>
+#include <plat/exti.h>
+#include <plat/gpio.h>
+#include <platform.h>
+#include <plat/syscfg.h>
+#include <plat/rtc.h>
+#include <sensors.h>
+#include <seos.h>
+#include <slab.h>
+#include <heap.h>
+#include <i2c.h>
+#include <timer.h>
+#include <variant/sensType.h>
+#include <cpu/cpuMath.h>
+#include <floatRt.h>
+
+#include <stdlib.h>
+#include <string.h>
+#include <variant/variant.h>
+
+#define ST_ACC44_APP_ID APP_ID_MAKE(NANOHUB_VENDOR_STMICRO, 7)
+
+/* Sensor registers */
+#define ST_ACC44_WAI_REG_ADDR 0x0F
+#define ST_ACC44_WAI_REG_VAL 0x44
+
+/*
+ * CTRL1 Register
+ *
+ * CTRL1[7:4] := ODR
+ * CTRL1[3:2] := MODE
+ * CTRL1[1:0] := LP_MODE
+ */
+#define ST_ACC44_CTRL1_REG 0x20
+#define ST_ACC44_ODR_POWER_DOWN 0x00
+#define ST_ACC44_ODR_12_5_HZ 0x20
+#define ST_ACC44_ODR_25_HZ 0x30
+#define ST_ACC44_ODR_50_HZ 0x40
+#define ST_ACC44_ODR_100_HZ 0x50
+#define ST_ACC44_ODR_200_HZ 0x60
+#define ST_ACC44_ODR_400_HZ 0x70
+#define ST_ACC44_ODR_800_HZ 0x80
+#define ST_ACC44_ODR_1600_HZ 0x90
+#define ST_ACC44_HIPERF_MODE 0x04
+#define ST_ACC44_CTRL1_DEFVAL (ST_ACC44_HIPERF_MODE)
+
+/*
+ * CTRL2 Register
+ *
+ * CTRL2[7] := BOOT
+ * CTRL2[6] := SOFT_RESET
+ * CTRL2[3] := BDU
+ */
+#define ST_ACC44_CTRL2_REG 0x21
+#define ST_ACC44_CTRL2_BOOT 0x80
+#define ST_ACC44_CTRL2_SW_RST 0x40
+#define ST_ACC44_CTRL2_BDU 0x08
+#define ST_ACC44_CTRL2_IF_ADD_INC 0x04
+#define ST_ACC44_CTRL2_DEFVAL (ST_ACC44_CTRL2_BDU | ST_ACC44_CTRL2_IF_ADD_INC)
+
+/*
+ * CTRL3 Register
+ */
+#define ST_ACC44_CTRL3_REG 0x22
+#define ST_ACC44_CTRL3_LIR 0x10
+
+/*
+ * CTRL4 Register
+ *
+ * CTRL4[7] := INT1_6D
+ * CTRL4[6] := INT1_SINGLE_TAP
+ * CTRL4[5] := INT1_WU
+ * CTRL4[4] := INT1_FF
+ * CTRL4[3] := INT1_TAP
+ * CTRL4[2] := INT1_DIFF5
+ * CTRL4[1] := INT1_FTH
+ * CTRL4[0] := INT1_DRDY
+ */
+#define ST_ACC44_CTRL4_REG 0x23
+#define ST_ACC44_CTRL4_INT1_6D 0x80
+#define ST_ACC44_CTRL4_INT1_STAP 0x40
+#define ST_ACC44_CTRL4_INT1_WU 0x20
+#define ST_ACC44_CTRL4_INT1_FF 0x10
+#define ST_ACC44_CTRL4_INT1_DTAP 0x08
+#define ST_ACC44_CTRL4_INT1_DIFF5 0x04
+#define ST_ACC44_CTRL4_INT1_FTH 0x02
+#define ST_ACC44_CTRL4_INT1_DRDY 0x01
+
+/*
+ * CTRL5 Register
+ */
+#define ST_ACC44_CTRL5_REG 0x24
+
+/*
+ * CTRL6 Register
+ *
+ * CTRL6[5:4] := FS
+ */
+#define ST_ACC44_CTRL6_REG 0x25
+#define ST_ACC44_CTRL6_FS_2G 0x00
+#define ST_ACC44_CTRL6_FS_4G 0x10
+#define ST_ACC44_CTRL6_FS_8G 0x20
+#define ST_ACC44_CTRL6_FS_16G 0x30
+
+/*
+ * STATUS Register
+ */
+#define ST_ACC44_STATUS_REG_ADDR 0x27
+#define ST_ACC44_STATUS_REG_FTH 0x80
+#define ST_ACC44_STATUS_REG_DRDY 0x01
+
+/*
+ * OUTXL Register
+ */
+#define ST_ACC44_OUTXL_REG_ADDR 0x28
+
+/*
+ * value in m/s2 per LSB (in high-resolution mode @8g)
+ * Since samples are 14-bit left aligned, the value
+ * must also be right-shifted by 2.
+ *
+ * (9.80665 * 0.976) / (4 * 1000)
+ */
+#define ST_ACC44_KSCALE 0.0023928226
+
+
+/* Enable auto-increment of the I2C subaddress (to allow I2C multiple ops) */
+#define ST_ACC44_I2C_AUTO_INCR 0x80
+
+#define INFO_PRINT(fmt, ...) \
+ do { \
+ osLog(LOG_INFO, "%s " fmt, "[ST_ACC44]", ##__VA_ARGS__); \
+ } while (0);
+
+#define DEBUG_PRINT(fmt, ...) \
+ do { \
+ if (ST_ACC44_DBG_ENABLED) { \
+ osLog(LOG_DEBUG, "%s " fmt, "[ST_ACC44]", ##__VA_ARGS__); \
+ } \
+ } while (0);
+
+#define ERROR_PRINT(fmt, ...) \
+ do { \
+ osLog(LOG_ERROR, "%s " fmt, "[ST_ACC44]", ##__VA_ARGS__); \
+ } while (0);
+
+/* DO NOT MODIFY, just to avoid compiler error if not defined using FLAGS */
+#ifndef ST_ACC44_DBG_ENABLED
+#define ST_ACC44_DBG_ENABLED 0
+#endif /* ST_ACC44_DBG_ENABLED */
+
+enum st_acc44_SensorEvents
+{
+ EVT_COMM_DONE = EVT_APP_START + 1,
+ EVT_SENSOR_INTERRUPT,
+};
+
+enum st_acc44_SensorState {
+ SENSOR_BOOT,
+ SENSOR_VERIFY_ID,
+ SENSOR_INIT,
+ SENSOR_IDLE,
+ SENSOR_ACCEL_POWER_UP,
+ SENSOR_ACCEL_POWER_DOWN,
+ SENSOR_CHANGE_RATE,
+ SENSOR_READ_SAMPLES,
+};
+
+#ifndef ST_ACC44_I2C_BUS_ID
+#error "ST_ACC44_I2C_BUS_ID is not defined; please define in variant.h"
+#endif
+
+#ifndef ST_ACC44_I2C_SPEED
+#error "ST_ACC44_I2C_SPEED is not defined; please define in variant.h"
+#endif
+
+#ifndef ST_ACC44_I2C_ADDR
+#error "ST_ACC44_I2C_ADDR is not defined; please define in variant.h"
+#endif
+
+#ifndef ST_ACC44_INT_PIN
+#error "ST_ACC44_INT_PIN is not defined; please define in variant.h"
+#endif
+
+#ifndef ST_ACC44_INT_IRQ
+#error "ST_ACC44_INT_IRQ is not defined; please define in variant.h"
+#endif
+
+#ifndef ST_ACC44_TO_ANDROID_COORDINATE
+#error "ST_ACC44_TO_ANDROID_COORDINATE is not defined; please define in variant.h"
+#endif
+
+#define RAW_TO_MS2(raw_axis) ((float)raw_axis * ST_ACC44_KSCALE)
+
+#define ST_ACC44_MAX_PENDING_I2C_REQUESTS 10
+#define ST_ACC44_MAX_I2C_TRANSFER_SIZE 6
+#define ST_ACC44_MAX_ACC_EVENTS 50
+
+struct I2cTransfer
+{
+ size_t tx;
+ size_t rx;
+ int err;
+ uint8_t txrxBuf[ST_ACC44_MAX_I2C_TRANSFER_SIZE];
+ bool last;
+ bool inUse;
+ uint32_t delay;
+};
+
+/* Task structure */
+struct st_acc44_Task {
+ uint32_t tid;
+
+ struct SlabAllocator *accDataSlab;
+
+ volatile uint8_t state; //task state, type enum st_mag40_SensorState, do NOT change this directly
+ bool accOn;
+ uint32_t sample_rate_ns;
+ uint32_t irq_rate_ns;
+ uint32_t rate;
+ uint32_t latency;
+ uint8_t currentODR;
+ uint8_t samplesToDiscard;
+ uint64_t Timestamp;
+ uint64_t lastTime;
+
+ bool pendingInt;
+ bool pendingSetPower;
+ bool pendingSetRate;
+ uint32_t pendingRate;
+ uint32_t pendingLatency;
+ bool pendingPower;
+
+ struct I2cTransfer transfers[ST_ACC44_MAX_PENDING_I2C_REQUESTS];
+
+ /* Communication functions */
+ bool (*comm_tx)(uint8_t addr, uint8_t data, uint32_t delay, bool last);
+ bool (*comm_rx)(uint8_t addr, uint16_t len, uint32_t delay, bool last);
+
+ /* irq */
+ struct Gpio *Int1;
+ struct ChainedIsr Isr1;
+ uint32_t int_num;
+
+ /* sensors */
+ uint32_t accHandle;
+};
+
+static struct st_acc44_Task mTask;
+
+#if DBG_STATE
+#define PRI_STATE PRIi32
+static int32_t getStateName(int32_t s) {
+ return s;
+}
+#endif
+
+// Atomic get state
+#define GET_STATE() (atomicReadByte(&mTask.state))
+
+// Atomic set state, this set the state to arbitrary value, use with caution
+#define SET_STATE(s) do{\
+ atomicWriteByte(&mTask.state, (s));\
+ }while(0)
+
+// Atomic switch state from IDLE to desired state.
+static bool trySwitchState(enum st_acc44_SensorState newState) {
+#if DBG_STATE
+ bool ret = atomicCmpXchgByte(&mTask.state, SENSOR_IDLE, newState);
+ uint8_t prevState = ret ? SENSOR_IDLE : GET_STATE();
+ DEBUG_PRINT("switch state %" PRI_STATE "->%" PRI_STATE ", %s\n",
+ getStateName(prevState), getStateName(newState), ret ? "ok" : "failed");
+ return ret;
+#else
+ return atomicCmpXchgByte(&mTask.state, SENSOR_IDLE, newState);
+#endif
+}
+
+#define DEC_INFO(name, type, axis, inter, samples, rates, raw, scale) \
+ .sensorName = name, \
+ .sensorType = type, \
+ .numAxis = axis, \
+ .interrupt = inter, \
+ .minSamples = samples, \
+ .supportedRates = rates, \
+ .rawType = raw, \
+ .rawScale = scale,
+
+static uint32_t st_acc44_Rates[] = {
+ SENSOR_HZ(25.0f/2.0f),
+ SENSOR_HZ(25.0f),
+ SENSOR_HZ(50.0f),
+ SENSOR_HZ(100.0f),
+ SENSOR_HZ(200.0f),
+ SENSOR_HZ(400.0f),
+ SENSOR_HZ(800.0f),
+ 0
+};
+
+static uint32_t st_acc44_Rates_in_ns[] = {
+ 80000000, /* 12.5 Hz */
+ 40000000, /* 25 Hz */
+ 20000000, /* 50 Hz */
+ 10000000, /* 100 Hz */
+ 5000000, /* 200 Hz */
+ 2500000, /* 400 Hz */
+ 1250000, /* 800 Hz */
+ 0
+};
+
+static uint32_t st_acc44_regVal[] = {
+ ST_ACC44_ODR_12_5_HZ,
+ ST_ACC44_ODR_25_HZ,
+ ST_ACC44_ODR_50_HZ,
+ ST_ACC44_ODR_100_HZ,
+ ST_ACC44_ODR_200_HZ,
+ ST_ACC44_ODR_400_HZ,
+ ST_ACC44_ODR_800_HZ,
+ ST_ACC44_ODR_1600_HZ,
+};
+
+static uint8_t st_acc44_computeOdr(uint32_t rate)
+{
+ int i;
+
+ for (i = 0; i < (ARRAY_SIZE(st_acc44_Rates) - 1); i++) {
+ if (st_acc44_Rates[i] == rate)
+ break;
+ }
+ if (i == (ARRAY_SIZE(st_acc44_Rates) -1 )) {
+ ERROR_PRINT("ODR not valid! Choosed smallest ODR available\n");
+ i = 0;
+ }
+
+ return i;
+}
+
+static uint32_t st_acc44_Rate_hz_to_ns(uint32_t rate)
+{
+ int i;
+
+ if ((i = st_acc44_computeOdr(rate)) >= 0)
+ return st_acc44_Rates_in_ns[i];
+
+ return 0;
+}
+
+static const struct SensorInfo st_acc44_SensorInfo =
+{
+ DEC_INFO("Accelerometer", SENS_TYPE_ACCEL, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP,
+ 600, st_acc44_Rates, SENS_TYPE_ACCEL_RAW, 1.0f / ST_ACC44_KSCALE)
+};
+
+static bool st_acc44_Power(bool on, void *cookie)
+{
+ bool oldMode = mTask.accOn;
+ bool newMode = on;
+ uint32_t state = on ? SENSOR_ACCEL_POWER_UP : SENSOR_ACCEL_POWER_DOWN;
+ bool ret = true;
+
+ INFO_PRINT("Power %s\n", on ? "on" : "off");
+
+ if (trySwitchState(state)) {
+ if (oldMode != newMode) {
+ if (on) {
+ ret = mTask.comm_tx(ST_ACC44_CTRL1_REG, ST_ACC44_ODR_12_5_HZ |
+ ST_ACC44_CTRL1_DEFVAL, 0, true);
+ } else {
+ ret = mTask.comm_tx(ST_ACC44_CTRL1_REG, ST_ACC44_ODR_POWER_DOWN |
+ ST_ACC44_CTRL1_DEFVAL, 0, true);
+ }
+ } else
+ sensorSignalInternalEvt(mTask.accHandle,
+ SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);
+ } else {
+ mTask.pendingSetPower = true;
+ mTask.pendingPower = on;
+ }
+
+ return ret;
+}
+
+static bool st_acc44_FwUpload(void *cookie)
+{
+ INFO_PRINT("FwUpload\n");
+ return sensorSignalInternalEvt(mTask.accHandle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
+}
+
+static bool st_acc44_SetRate(uint32_t rate, uint64_t latency, void *cookie)
+{
+ uint8_t num = 0;
+
+ INFO_PRINT("SetRate %lu Hz - %llu ns\n", rate, latency);
+
+ if (trySwitchState(SENSOR_CHANGE_RATE)) {
+ num = st_acc44_computeOdr(rate);
+ mTask.currentODR = st_acc44_regVal[num];
+ mTask.latency = latency;
+ mTask.rate = rate;
+ mTask.sample_rate_ns = st_acc44_Rate_hz_to_ns(rate);
+ mTask.samplesToDiscard = 2;
+ mTask.lastTime = 0;
+
+ /* one interrupt every sample */
+ mTask.irq_rate_ns = mTask.sample_rate_ns;
+
+ mTask.comm_rx(ST_ACC44_OUTXL_REG_ADDR, 6, 0, false);
+ mTask.comm_tx(ST_ACC44_CTRL4_REG, ST_ACC44_CTRL4_INT1_DRDY, 0, false);
+ mTask.comm_tx(ST_ACC44_CTRL1_REG, mTask.currentODR | ST_ACC44_CTRL1_DEFVAL, 0, true);
+ } else {
+ mTask.pendingSetRate = true;
+ mTask.pendingRate = rate;
+ mTask.pendingLatency = latency;
+ }
+
+ return true;
+}
+
+static bool st_acc44_Flush(void *cookie)
+{
+ INFO_PRINT("Flush\n");
+ return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_ACCEL), SENSOR_DATA_EVENT_FLUSH, NULL);
+}
+
+static bool st_acc44_SelfTest(void *cookie)
+{
+ INFO_PRINT("SelfTest\n");
+ return true;
+}
+
+#define DEC_OPS(power, firmware, rate, flush, test, cal, cfg) \
+ .sensorPower = power, \
+ .sensorFirmwareUpload = firmware, \
+ .sensorSetRate = rate, \
+ .sensorFlush = flush, \
+ .sensorCalibrate = cal, \
+ .sensorSelfTest = test, \
+ .sensorCfgData = cfg
+
+static const struct SensorOps st_acc44_SensorOps =
+{
+ DEC_OPS(st_acc44_Power, st_acc44_FwUpload, st_acc44_SetRate, st_acc44_Flush, st_acc44_SelfTest, NULL, NULL),
+};
+
+static void inline enableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
+{
+ gpioConfigInput(pin, GPIO_SPEED_LOW, GPIO_PULL_NONE);
+ syscfgSetExtiPort(pin);
+ extiEnableIntGpio(pin, EXTI_TRIGGER_RISING);
+ extiChainIsr(ST_ACC44_INT_IRQ, isr);
+}
+
+static void inline disableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
+{
+ extiUnchainIsr(ST_ACC44_INT_IRQ, isr);
+ extiDisableIntGpio(pin);
+}
+
+static void st_acc44_calc_timestamp(void)
+{
+ if (mTask.lastTime == 0) {
+ mTask.Timestamp = sensorGetTime();
+ } else {
+ uint64_t currTime = sensorGetTime();
+ uint64_t deltaTime = currTime - mTask.lastTime;
+
+ deltaTime = (deltaTime + 7*mTask.irq_rate_ns)/8;
+ mTask.Timestamp = mTask.lastTime + deltaTime;
+ }
+ mTask.lastTime = mTask.Timestamp;
+}
+
+static bool st_acc44_int1_isr(struct ChainedIsr *isr)
+{
+ if (!extiIsPendingGpio(mTask.Int1))
+ return false;
+
+ /* Start sampling for a value */
+ if (!osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT, NULL, NULL, mTask.tid))
+ ERROR_PRINT("st_acc44_int1_isr: osEnqueuePrivateEvt() failed\n");
+
+ mTask.int_num++;
+ extiClearPendingGpio(mTask.Int1);
+ return true;
+}
+
+static void int2Evt(void)
+{
+ if (trySwitchState(SENSOR_READ_SAMPLES)) {
+ mTask.comm_rx(ST_ACC44_OUTXL_REG_ADDR, 6, 0, true);
+ } else {
+ mTask.pendingInt = true;
+ }
+}
+
+static void processPendingEvt(void)
+{
+ if (mTask.pendingInt) {
+ mTask.pendingInt = false;
+ int2Evt();
+ return;
+ }
+
+ if (mTask.pendingSetPower) {
+ mTask.pendingSetPower = false;
+ st_acc44_Power(mTask.pendingPower, NULL);
+ }
+
+ if (mTask.pendingSetRate) {
+ mTask.pendingSetRate = false;
+ st_acc44_SetRate(mTask.pendingRate, mTask.pendingLatency, NULL);
+ }
+}
+
+static bool accAllocateEvt(struct TripleAxisDataEvent **evPtr)
+{
+ struct TripleAxisDataEvent *ev;
+
+ ev = *evPtr = slabAllocatorAlloc(mTask.accDataSlab);
+ if (!ev) {
+ ERROR_PRINT("Failed to allocate acc event memory");
+ return false;
+ }
+
+ memset(&ev->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample));
+ return true;
+}
+
+static void accFreeEvt(void *ptr)
+{
+ slabAllocatorFree(mTask.accDataSlab, ptr);
+}
+
+// Allocate a buffer and mark it as in use with the given state, or return NULL
+// if no buffers available. Must *not* be called from interrupt context.
+static struct I2cTransfer *allocXfer(void)
+{
+ size_t i;
+
+ for (i = 0; i < ARRAY_SIZE(mTask.transfers); i++) {
+ if (!mTask.transfers[i].inUse) {
+ mTask.transfers[i].inUse = true;
+ return &mTask.transfers[i];
+ }
+ }
+
+ ERROR_PRINT("Ran out of i2c buffers!");
+ return NULL;
+}
+
+static inline void releaseXfer(struct I2cTransfer *xfer)
+{
+ xfer->inUse = false;
+}
+
+static void st_acc44_i2cCallback(void *cookie, size_t tx, size_t rx, int err)
+{
+ struct I2cTransfer *xfer = cookie;
+
+ /* Do not run callback if not the last one in a set of i2c transfers */
+ if (xfer && !xfer->last) {
+ releaseXfer(xfer);
+ return;
+ }
+
+ xfer->tx = tx;
+ xfer->rx = rx;
+ xfer->err = err;
+
+ osEnqueuePrivateEvt(EVT_COMM_DONE, cookie, NULL, mTask.tid);
+ if (err != 0)
+ ERROR_PRINT("i2c error (tx: %d, rx: %d, err: %d)\n", tx, rx, err);
+}
+
+static bool st_acc44_i2c_read(uint8_t addr, uint16_t len, uint32_t delay, bool last)
+{
+ struct I2cTransfer *xfer = allocXfer();
+ int ret = -1;
+
+ if (xfer != NULL) {
+ xfer->delay = delay;
+ xfer->last = last;
+ xfer->txrxBuf[0] = ST_ACC44_I2C_AUTO_INCR | addr;
+ if ((ret = i2cMasterTxRx(ST_ACC44_I2C_BUS_ID, ST_ACC44_I2C_ADDR, xfer->txrxBuf, 1, xfer->txrxBuf, len, st_acc44_i2cCallback, xfer)) < 0) {
+ releaseXfer(xfer);
+ DEBUG_PRINT("st_acc44_i2c_read: i2cMasterTxRx operation failed (ret: %d)\n", ret);
+ return false;
+ }
+ }
+
+ return (ret == -1) ? false : true;
+}
+
+static bool st_acc44_i2c_write(uint8_t addr, uint8_t data, uint32_t delay, bool last)
+{
+ struct I2cTransfer *xfer = allocXfer();
+ int ret = -1;
+
+ if (xfer != NULL) {
+ xfer->delay = delay;
+ xfer->last = last;
+ xfer->txrxBuf[0] = addr;
+ xfer->txrxBuf[1] = data;
+ if ((ret = i2cMasterTx(ST_ACC44_I2C_BUS_ID, ST_ACC44_I2C_ADDR, xfer->txrxBuf, 2, st_acc44_i2cCallback, xfer)) < 0) {
+ releaseXfer(xfer);
+ DEBUG_PRINT("st_acc44_i2c_write: i2cMasterTx operation failed (ret: %d)\n", ret);
+ return false;
+ }
+ }
+
+ return (ret == -1) ? false : true;
+}
+
+static void parseRawData(uint8_t *raw, uint8_t num_of_smpl, uint64_t sensor_time)
+{
+ uint8_t i;
+ struct TripleAxisDataEvent *accSample;
+ float x, y, z;
+ int32_t raw_x;
+ int32_t raw_y;
+ int32_t raw_z;
+
+ /* Discard samples generated during sensor turn-on time */
+ if (mTask.samplesToDiscard > 0) {
+ if (num_of_smpl > mTask.samplesToDiscard) {
+ num_of_smpl -= mTask.samplesToDiscard;
+ mTask.samplesToDiscard = 0;
+ } else{
+ mTask.samplesToDiscard -= num_of_smpl;
+ return;
+ }
+ }
+
+ if (accAllocateEvt(&accSample) == false)
+ return;
+
+ accSample->referenceTime = sensor_time;
+
+ accSample->samples[0].deltaTime = 0;
+ accSample->samples[0].firstSample.numSamples = num_of_smpl;
+
+ for (i = 0; i < num_of_smpl; i++) {
+ raw_x = (*(int16_t *)&raw[6*i + 0]);
+ raw_y = (*(int16_t *)&raw[6*i + 2]);
+ raw_z = (*(int16_t *)&raw[6*i + 4]);
+
+ /* convert raw data in m/s2 */
+ x = RAW_TO_MS2(raw_x);
+ y = RAW_TO_MS2(raw_y);
+ z = RAW_TO_MS2(raw_z);
+
+ /* rotate axis */
+ ST_ACC44_TO_ANDROID_COORDINATE(x, y, z);
+
+ accSample->samples[i].x = x;
+ accSample->samples[i].y = y;
+ accSample->samples[i].z = z;
+
+ if (i > 0)
+ accSample->samples[i].deltaTime = mTask.sample_rate_ns;
+ }
+
+ osEnqueueEvtOrFree(sensorGetMyEventType(SENS_TYPE_ACCEL), accSample, accFreeEvt);
+}
+
+static int st_acc44_handleCommDoneEvt(const void* evtData)
+{
+ bool returnIdle = false;
+ struct I2cTransfer *xfer = (struct I2cTransfer *)evtData;
+
+ switch (GET_STATE()) {
+ case SENSOR_BOOT:
+ SET_STATE(SENSOR_VERIFY_ID);
+ if (!mTask.comm_rx(ST_ACC44_WAI_REG_ADDR, 1, 0, true)) {
+ DEBUG_PRINT("Not able to read WAI\n");
+ return -1;
+ }
+ break;
+
+ case SENSOR_VERIFY_ID:
+ /* Check the sensor ID */
+ if (xfer->err != 0 || xfer->txrxBuf[0] != ST_ACC44_WAI_REG_VAL) {
+ DEBUG_PRINT("WAI returned is: %02x\n", xfer->txrxBuf[0]);
+ break;
+ }
+
+ INFO_PRINT("Device ID is correct! (%02x)\n", xfer->txrxBuf[0]);
+
+ SET_STATE(SENSOR_INIT);
+ mTask.comm_tx(ST_ACC44_CTRL1_REG, ST_ACC44_ODR_POWER_DOWN | ST_ACC44_CTRL1_DEFVAL, 0, false);
+ mTask.comm_tx(ST_ACC44_CTRL2_REG, ST_ACC44_CTRL2_DEFVAL, 0, false);
+ mTask.comm_tx(ST_ACC44_CTRL3_REG, ST_ACC44_CTRL3_LIR, 0, false);
+ mTask.comm_tx(ST_ACC44_CTRL6_REG, ST_ACC44_CTRL6_FS_8G, 0, false);
+ mTask.comm_tx(ST_ACC44_CTRL4_REG, 0, 0, true);
+ break;
+
+ case SENSOR_INIT:
+ DEBUG_PRINT("SENSOR INIT\n");
+ returnIdle = true;
+ sensorRegisterInitComplete(mTask.accHandle);
+ break;
+
+ case SENSOR_ACCEL_POWER_UP:
+ DEBUG_PRINT("POWER UP\n");
+ returnIdle = true;
+ mTask.accOn = true;
+ sensorSignalInternalEvt(mTask.accHandle,
+ SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0);
+ break;
+
+ case SENSOR_ACCEL_POWER_DOWN:
+ DEBUG_PRINT("POWER DWN\n");
+ returnIdle = true;
+ mTask.accOn = false;
+ sensorSignalInternalEvt(mTask.accHandle,
+ SENSOR_INTERNAL_EVT_POWER_STATE_CHG, false, 0);
+ break;
+
+ case SENSOR_CHANGE_RATE:
+ DEBUG_PRINT("CHANGE RATE\n");
+ returnIdle = true;
+ DEBUG_PRINT("int_num %ld\n", mTask.int_num);
+ mTask.int_num = 0;
+ sensorSignalInternalEvt(mTask.accHandle,
+ SENSOR_INTERNAL_EVT_RATE_CHG, mTask.rate, mTask.latency);
+ break;
+
+ case SENSOR_READ_SAMPLES:
+ returnIdle = true;
+
+ parseRawData(&xfer->txrxBuf[0], 1, mTask.Timestamp);
+ break;
+
+ case SENSOR_IDLE:
+ default:
+ break;
+ }
+
+ releaseXfer(xfer);
+
+ if (returnIdle) {
+ SET_STATE(SENSOR_IDLE);
+ processPendingEvt();
+ }
+
+ return (0);
+}
+
+static void st_acc44_handleEvent(uint32_t evtType, const void* evtData)
+{
+ switch (evtType) {
+ case EVT_APP_START:
+ INFO_PRINT("EVT_APP_START\n");
+ osEventUnsubscribe(mTask.tid, EVT_APP_START);
+
+ SET_STATE(SENSOR_BOOT);
+ mTask.comm_tx(ST_ACC44_CTRL2_REG, ST_ACC44_CTRL2_SW_RST, 0, true);
+ break;
+
+ case EVT_COMM_DONE:
+ st_acc44_handleCommDoneEvt(evtData);
+ break;
+
+ case EVT_SENSOR_INTERRUPT:
+ st_acc44_calc_timestamp();
+ int2Evt();
+ break;
+
+ default:
+ break;
+ }
+}
+
+static bool st_acc44_startTask(uint32_t task_id)
+{
+ size_t slabSize;
+
+ mTask.tid = task_id;
+
+ INFO_PRINT("start driver\n");
+
+ mTask.accOn = false;
+ mTask.pendingInt = false;
+ mTask.pendingSetPower = false;
+ mTask.pendingSetRate = false;
+
+ mTask.currentODR = ST_ACC44_ODR_POWER_DOWN;
+
+ slabSize = sizeof(struct TripleAxisDataEvent) + sizeof(struct TripleAxisDataPoint);
+
+ mTask.accDataSlab = slabAllocatorNew(slabSize, 4, ST_ACC44_MAX_ACC_EVENTS);
+ if (!mTask.accDataSlab) {
+ ERROR_PRINT("Failed to allocate accDataSlab memory\n");
+ return false;
+ }
+
+ /* Init the communication part */
+ i2cMasterRequest(ST_ACC44_I2C_BUS_ID, ST_ACC44_I2C_SPEED);
+
+ mTask.comm_tx = st_acc44_i2c_write;
+ mTask.comm_rx = st_acc44_i2c_read;
+
+ /* irq */
+ mTask.int_num = 0;
+ mTask.Int1 = gpioRequest(ST_ACC44_INT_PIN);
+ gpioConfigInput(mTask.Int1, GPIO_SPEED_LOW, GPIO_PULL_NONE);
+ mTask.Isr1.func = st_acc44_int1_isr;
+ enableInterrupt(mTask.Int1, &mTask.Isr1);
+
+ mTask.accHandle = sensorRegister(&st_acc44_SensorInfo, &st_acc44_SensorOps, NULL, false);
+
+ osEventSubscribe(mTask.tid, EVT_APP_START);
+
+ return true;
+}
+
+static void st_acc44_endTask(void)
+{
+ INFO_PRINT("ended\n");
+ slabAllocatorDestroy(mTask.accDataSlab);
+ disableInterrupt(mTask.Int1, &mTask.Isr1);
+}
+
+INTERNAL_APP_INIT(ST_ACC44_APP_ID, 0, st_acc44_startTask, st_acc44_endTask, st_acc44_handleEvent);
