xs-android13
/
device
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
device/google/contexthub/firmware/os/drivers/st_acc44/st_acc44.c

844 lines
22 KiB
C
Raw Normal View History

android 13 from xiaosuan
2025-08-25 08:28:21 +08:00
/*
* 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);