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

1074 lines
31 KiB
C
Raw Normal View History

android 13 from xiaosuan
2025-08-25 08:28:21 +08:00
/*
* Copyright (C) 2016 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 <halIntf.h>
#include <slab.h>
#include <heap.h>
#include <i2c.h>
#include <timer.h>
#include <variant/sensType.h>
#include <cpu/cpuMath.h>
#include <calibration/magnetometer/mag_cal.h>
#include <floatRt.h>
#include <stdlib.h>
#include <string.h>
#include <variant/variant.h>
#define ST_MAG40_APP_ID APP_ID_MAKE(NANOHUB_VENDOR_STMICRO, 3)
/* Sensor registers */
#define ST_MAG40_WAI_REG_ADDR 0x4F
#define ST_MAG40_WAI_REG_VAL 0x40
#define ST_MAG40_CFG_A_REG_ADDR 0x60
#define ST_MAG40_TEMP_COMP_EN 0x80
#define ST_MAG40_SOFT_RESET_BIT 0x20
#define ST_MAG40_ODR_10_HZ 0x00
#define ST_MAG40_ODR_20_HZ 0x04
#define ST_MAG40_ODR_50_HZ 0x08
#define ST_MAG40_ODR_100_HZ 0x0C
#define ST_MAG40_POWER_ON 0x00
#define ST_MAG40_POWER_IDLE 0x03
#define ST_MAG40_CFG_B_REG_ADDR 0x61
#define ST_MAG40_OFF_CANC 0x02
#define ST_MAG40_CFG_C_REG_ADDR 0x62
#define ST_MAG40_I2C_DIS 0x20
#define ST_MAG40_BDU_ON 0x10
#define ST_MAG40_SELFTEST_EN 0x02
#define ST_MAG40_INT_MAG 0x01
#define ST_MAG40_OUTXL_REG_ADDR 0x68
/* Enable auto-increment of the I2C subaddress (to allow I2C multiple ops) */
#define ST_MAG40_I2C_AUTO_INCR 0x80
enum st_mag40_SensorEvents
{
EVT_COMM_DONE = EVT_APP_START + 1,
EVT_SENSOR_INTERRUPT,
};
enum st_mag40_TestState {
MAG_SELFTEST_INIT,
MAG_SELFTEST_RUN_ST_OFF,
MAG_SELFTEST_INIT_ST_EN,
MAG_SELFTEST_RUN_ST_ON,
MAG_SELFTEST_VERIFY,
MAG_SELFTEST_DONE,
};
enum st_mag40_SensorState {
SENSOR_BOOT,
SENSOR_VERIFY_ID,
SENSOR_INITIALIZATION,
SENSOR_IDLE,
SENSOR_MAG_CONFIGURATION,
SENSOR_READ_SAMPLES,
SENSOR_SELF_TEST,
};
enum st_mag40_subState {
NO_SUBSTATE = 0,
INIT_START,
INIT_ENABLE_DRDY,
INIT_I2C_DISABLE_ACCEL,
INIT_DONE,
CONFIG_POWER_UP,
CONFIG_POWER_UP_2,
CONFIG_POWER_DOWN,
CONFIG_POWER_DOWN_2,
CONFIG_SET_RATE,
CONFIG_SET_RATE_2,
CONFIG_DONE,
};
struct TestResultData {
struct HostHubRawPacket header;
struct SensorAppEventHeader data_header;
} __attribute__((packed));
#ifndef ST_MAG40_I2C_BUS_ID
#error "ST_MAG40_I2C_BUS_ID is not defined; please define in variant.h"
#endif
#ifndef ST_MAG40_I2C_SPEED
#error "ST_MAG40_I2C_SPEED is not defined; please define in variant.h"
#endif
#ifndef ST_MAG40_I2C_ADDR
#error "ST_MAG40_I2C_ADDR is not defined; please define in variant.h"
#endif
#ifndef ST_MAG40_INT_PIN
#error "ST_MAG40_INT_PIN is not defined; please define in variant.h"
#endif
#ifndef ST_MAG40_INT_IRQ
#error "ST_MAG40_INT_IRQ is not defined; please define in variant.h"
#endif
#ifndef ST_MAG40_ROT_MATRIX
#error "ST_MAG40_ROT_MATRIX is not defined; please define in variant.h"
#endif
#define ST_MAG40_X_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \
((r11 == 1 ? x : (r11 == -1 ? -x : 0)) + \
(r12 == 1 ? y : (r12 == -1 ? -y : 0)) + \
(r13 == 1 ? z : (r13 == -1 ? -z : 0)))
#define ST_MAG40_Y_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \
((r21 == 1 ? x : (r21 == -1 ? -x : 0)) + \
(r22 == 1 ? y : (r22 == -1 ? -y : 0)) + \
(r23 == 1 ? z : (r23 == -1 ? -z : 0)))
#define ST_MAG40_Z_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \
((r31 == 1 ? x : (r31 == -1 ? -x : 0)) + \
(r32 == 1 ? y : (r32 == -1 ? -y : 0)) + \
(r33 == 1 ? z : (r33 == -1 ? -z : 0)))
#define ST_MAG40_REMAP_X_DATA(...) ST_MAG40_X_MAP(__VA_ARGS__)
#define ST_MAG40_REMAP_Y_DATA(...) ST_MAG40_Y_MAP(__VA_ARGS__)
#define ST_MAG40_REMAP_Z_DATA(...) ST_MAG40_Z_MAP(__VA_ARGS__)
/* Self Test macros */
#define ST_MAG40_ST_NUM_OF_SAMPLES 50
#define ST_MAG40_ST_MIN_THRESHOLD 10 /* 15 mGa */
#define ST_MAG40_ST_MAX_THRESHOLD 333 /* 500 mGa */
#define INFO_PRINT(fmt, ...) \
do { \
osLog(LOG_INFO, "%s " fmt, "[ST_MAG40]", ##__VA_ARGS__); \
} while (0);
#define DEBUG_PRINT(fmt, ...) \
do { \
if (ST_MAG40_DBG_ENABLED) { \
osLog(LOG_DEBUG, "%s " fmt, "[ST_MAG40]", ##__VA_ARGS__); \
} \
} while (0);
#define ERROR_PRINT(fmt, ...) \
do { \
osLog(LOG_ERROR, "%s " fmt, "[ST_MAG40]", ##__VA_ARGS__); \
} while (0);
/* DO NOT MODIFY, just to avoid compiler error if not defined using FLAGS */
#ifndef ST_MAG40_DBG_ENABLED
#define ST_MAG40_DBG_ENABLED 0
#endif /* ST_MAG40_DBG_ENABLED */
#define ST_MAG40_MAX_PENDING_I2C_REQUESTS 4
#define ST_MAG40_MAX_I2C_TRANSFER_SIZE 6
#define ST_MAG40_MAX_MAG_EVENTS 20
struct I2cTransfer
{
size_t tx;
size_t rx;
int err;
uint8_t txrxBuf[ST_MAG40_MAX_I2C_TRANSFER_SIZE];
bool last;
bool inUse;
uint32_t delay;
};
/* Task structure */
struct st_mag40_Task {
uint32_t tid;
struct SlabAllocator *magDataSlab;
uint64_t timestampInt;
volatile uint8_t state; //task state, type enum st_mag40_SensorState, do NOT change this directly
uint8_t subState;
/* sensor flags */
uint8_t samplesToDiscard;
uint32_t rate;
uint64_t latency;
bool magOn;
bool pendingInt;
uint8_t pendingSubState;
uint8_t currentODR;
#if defined(ST_MAG40_CAL_ENABLED)
struct MagCal moc;
#endif
unsigned char sens_buf[7];
struct I2cTransfer transfers[ST_MAG40_MAX_PENDING_I2C_REQUESTS];
/* Communication functions */
void (*comm_tx)(uint8_t addr, uint8_t data, uint32_t delay, bool last);
void (*comm_rx)(uint8_t addr, uint16_t len, uint32_t delay, bool last);
/* irq */
struct Gpio *Int1;
struct ChainedIsr Isr1;
/* Self Test */
enum st_mag40_TestState mag_test_state;
uint32_t mag_selftest_num;
int32_t dataST[3];
int32_t dataNOST[3];
/* sensors */
uint32_t magHandle;
};
static struct st_mag40_Task mTask;
static void sensorMagConfig(void);
#define PRI_STATE PRIi32
static int32_t getStateName(int32_t s) {
return s;
}
// 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{\
DEBUG_PRINT("set state %" PRI_STATE "\n", getStateName(s));\
atomicWriteByte(&mTask.state, (s));\
}while(0)
// Atomic switch state from IDLE to desired state.
static bool trySwitchState(enum st_mag40_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
}
static bool magAllocateEvt(struct TripleAxisDataEvent **evPtr)
{
struct TripleAxisDataEvent *ev;
ev = *evPtr = slabAllocatorAlloc(mTask.magDataSlab);
if (!ev) {
ERROR_PRINT("Failed to allocate mag event memory");
return false;
}
memset(&ev->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample));
return true;
}
static void magFreeEvt(void *ptr)
{
slabAllocatorFree(mTask.magDataSlab, 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 i2cCallback(void *cookie, size_t tx, size_t rx, int err);
/* delayed callback */
static void i2cDelayCallback(uint32_t timerId, void *data)
{
struct I2cTransfer *xfer = data;
i2cCallback((void *)xfer, xfer->tx, xfer->rx, xfer->err);
}
static void 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;
}
/* delay callback if it is the case */
if (xfer->delay > 0) {
xfer->tx = tx;
xfer->rx = rx;
xfer->err = err;
if (!timTimerSet(xfer->delay * 1000, 0, 50, i2cDelayCallback, xfer, true)) {
ERROR_PRINT("Cannot do delayed i2cCallback\n");
goto handle_now;
}
xfer->delay = 0;
return;
}
handle_now:
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 void i2c_read(uint8_t addr, uint16_t len, uint32_t delay, bool last)
{
struct I2cTransfer *xfer = allocXfer();
if (xfer != NULL) {
xfer->delay = delay;
xfer->last = last;
xfer->txrxBuf[0] = ST_MAG40_I2C_AUTO_INCR | addr;
i2cMasterTxRx(ST_MAG40_I2C_BUS_ID, ST_MAG40_I2C_ADDR, xfer->txrxBuf, 1, xfer->txrxBuf, len, i2cCallback, xfer);
}
}
static void i2c_write(uint8_t addr, uint8_t data, uint32_t delay, bool last)
{
struct I2cTransfer *xfer = allocXfer();
if (xfer != NULL) {
xfer->delay = delay;
xfer->last = last;
xfer->txrxBuf[0] = addr;
xfer->txrxBuf[1] = data;
i2cMasterTx(ST_MAG40_I2C_BUS_ID, ST_MAG40_I2C_ADDR, xfer->txrxBuf, 2, i2cCallback, xfer);
}
}
#define DEC_INFO_BIAS(name, type, axis, inter, samples, rates, raw, scale, bias) \
.sensorName = name, \
.sensorType = type, \
.numAxis = axis, \
.interrupt = inter, \
.minSamples = samples, \
.supportedRates = rates, \
.rawType = raw, \
.rawScale = scale, \
.biasType = bias
#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_mag40_Rates[] = {
SENSOR_HZ(10.0f),
SENSOR_HZ(20.0f),
SENSOR_HZ(50.0f),
SENSOR_HZ(100.0f),
0
};
static uint32_t st_mag40_regVal[] = {
ST_MAG40_ODR_10_HZ,
ST_MAG40_ODR_20_HZ,
ST_MAG40_ODR_50_HZ,
ST_MAG40_ODR_100_HZ,
};
static uint8_t st_mag40_computeOdr(uint32_t rate)
{
int i;
for (i = 0; i < (ARRAY_SIZE(st_mag40_Rates) - 1); i++) {
if (st_mag40_Rates[i] == rate)
break;
}
if (i == (ARRAY_SIZE(st_mag40_Rates) -1 )) {
ERROR_PRINT("ODR not valid! Choosed smallest ODR available\n");
i = 0;
}
return i;
}
static const struct SensorInfo st_mag40_SensorInfo =
{
#if defined(ST_MAG40_CAL_ENABLED)
DEC_INFO_BIAS("Magnetometer", SENS_TYPE_MAG, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP,
600, st_mag40_Rates, 0, 0, SENS_TYPE_MAG_BIAS)
#else
DEC_INFO("Magnetometer", SENS_TYPE_MAG, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP,
600, st_mag40_Rates, 0, 0)
#endif
};
/* Sensor Operations */
static bool magPower(bool on, void *cookie)
{
INFO_PRINT("magPower %s\n", on ? "on" : "off");
if (trySwitchState(SENSOR_MAG_CONFIGURATION)) {
mTask.subState = on ? CONFIG_POWER_UP : CONFIG_POWER_DOWN;
sensorMagConfig();
} else {
mTask.pendingSubState = on ? CONFIG_POWER_UP : CONFIG_POWER_DOWN;
}
return true;
}
static bool magFwUpload(void *cookie)
{
return sensorSignalInternalEvt(mTask.magHandle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
}
static bool magSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
uint8_t num = 0;
INFO_PRINT("magSetRate %lu Hz - %llu ns\n", rate, latency);
num = st_mag40_computeOdr(rate);
mTask.currentODR = st_mag40_regVal[num];
mTask.rate = rate;
mTask.latency = latency;
mTask.samplesToDiscard = 2;
if (trySwitchState(SENSOR_MAG_CONFIGURATION)) {
mTask.subState = CONFIG_SET_RATE;
sensorMagConfig();
} else {
mTask.pendingSubState = CONFIG_SET_RATE;
}
return true;
}
static bool magFlush(void *cookie)
{
INFO_PRINT("magFlush\n");
return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_MAG), SENSOR_DATA_EVENT_FLUSH, NULL);
}
static bool magCfgData(void *data, void *cookie)
{
#if defined(ST_MAG40_CAL_ENABLED)
const struct AppToSensorHalDataPayload *p = data;
if (p->type == HALINTF_TYPE_MAG_CAL_BIAS && p->size == sizeof(struct MagCalBias)) {
const struct MagCalBias *d = p->magCalBias;
INFO_PRINT("magCfgData: calibration %ldnT, %ldnT, %ldnT\n",
(int32_t)(d->bias[0] * 1000),
(int32_t)(d->bias[1] * 1000),
(int32_t)(d->bias[2] * 1000));
mTask.moc.x_bias = d->bias[0];
mTask.moc.y_bias = d->bias[1];
mTask.moc.z_bias = d->bias[2];
} else if (p->type == HALINTF_TYPE_MAG_LOCAL_FIELD && p->size == sizeof(struct MagLocalField)) {
const struct MagLocalField *d = p->magLocalField;
INFO_PRINT("magCfgData: local field strength %dnT, dec %ddeg, inc %ddeg\n",
(int)(d->strength * 1000),
(int)(d->declination * 180 / M_PI + 0.5f),
(int)(d->inclination * 180 / M_PI + 0.5f));
// Passing local field information to mag calibration routine
#ifdef DIVERSITY_CHECK_ENABLED
diversityCheckerLocalFieldUpdate(&mTask.moc.diversity_checker, d->strength);
#endif
// TODO: pass local field information to rotation vector sensor.
} else {
ERROR_PRINT("magCfgData: unknown type 0x%04x, size %d", p->type, p->size);
}
#endif /* ST_MAG40_CAL_ENABLED */
return true;
}
static void sendTestResult(uint8_t status, uint8_t sensorType)
{
struct TestResultData *data = heapAlloc(sizeof(struct TestResultData));
if (!data) {
ERROR_PRINT("Couldn't alloc test result packet");
return;
}
data->header.appId = ST_MAG40_APP_ID;
data->header.dataLen = (sizeof(struct TestResultData) - sizeof(struct HostHubRawPacket));
data->data_header.msgId = SENSOR_APP_MSG_ID_TEST_RESULT;
data->data_header.sensorType = sensorType;
data->data_header.status = status;
if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree))
ERROR_PRINT("Couldn't send test result packet");
}
static void magTestHandling(struct I2cTransfer *xfer)
{
int32_t dataGap[3];
switch(mTask.mag_test_state) {
case MAG_SELFTEST_INIT:
mTask.mag_selftest_num = 0;
memset(mTask.dataNOST, 0, 3 * sizeof(int32_t));
mTask.mag_test_state = MAG_SELFTEST_RUN_ST_OFF;
mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR, ST_MAG40_ODR_100_HZ, 0, false);
mTask.comm_tx(ST_MAG40_CFG_B_REG_ADDR, ST_MAG40_OFF_CANC, 0, false);
mTask.comm_tx(ST_MAG40_CFG_C_REG_ADDR, ST_MAG40_BDU_ON, 0, true);
break;
case MAG_SELFTEST_RUN_ST_OFF:
if (mTask.mag_selftest_num++ > 0) {
uint8_t *raw = &xfer->txrxBuf[0];
mTask.dataNOST[0] += (*(int16_t *)&raw[0]);
mTask.dataNOST[1] += (*(int16_t *)&raw[2]);
mTask.dataNOST[2] += (*(int16_t *)&raw[4]);
}
if (mTask.mag_selftest_num <= ST_MAG40_ST_NUM_OF_SAMPLES) {
mTask.comm_rx(ST_MAG40_OUTXL_REG_ADDR, 6, 10000, true);
break;
}
mTask.dataNOST[0] /= ST_MAG40_ST_NUM_OF_SAMPLES;
mTask.dataNOST[1] /= ST_MAG40_ST_NUM_OF_SAMPLES;
mTask.dataNOST[2] /= ST_MAG40_ST_NUM_OF_SAMPLES;
mTask.mag_test_state = MAG_SELFTEST_INIT_ST_EN;
/* fall through */
case MAG_SELFTEST_INIT_ST_EN:
mTask.mag_selftest_num = 0;
memset(mTask.dataST, 0, 3 * sizeof(int32_t));
mTask.mag_test_state = MAG_SELFTEST_RUN_ST_ON;
mTask.comm_tx(ST_MAG40_CFG_C_REG_ADDR, ST_MAG40_BDU_ON | ST_MAG40_SELFTEST_EN, 0, true);
break;
case MAG_SELFTEST_RUN_ST_ON:
if (mTask.mag_selftest_num++ > 0) {
uint8_t *raw = &xfer->txrxBuf[0];
mTask.dataST[0] += (*(int16_t *)&raw[0]);
mTask.dataST[1] += (*(int16_t *)&raw[2]);
mTask.dataST[2] += (*(int16_t *)&raw[4]);
}
if (mTask.mag_selftest_num <= ST_MAG40_ST_NUM_OF_SAMPLES) {
mTask.comm_rx(ST_MAG40_OUTXL_REG_ADDR, 6, 10000, true);
break;
}
mTask.dataST[0] /= ST_MAG40_ST_NUM_OF_SAMPLES;
mTask.dataST[1] /= ST_MAG40_ST_NUM_OF_SAMPLES;
mTask.dataST[2] /= ST_MAG40_ST_NUM_OF_SAMPLES;
mTask.mag_test_state = MAG_SELFTEST_VERIFY;
/* fall through */
case MAG_SELFTEST_VERIFY:
dataGap[0] = abs(mTask.dataST[0] - mTask.dataNOST[0]);
dataGap[1] = abs(mTask.dataST[1] - mTask.dataNOST[1]);
dataGap[2] = abs(mTask.dataST[2] - mTask.dataNOST[2]);
if (dataGap[0] >= ST_MAG40_ST_MIN_THRESHOLD &&
dataGap[0] <= ST_MAG40_ST_MAX_THRESHOLD &&
dataGap[1] >= ST_MAG40_ST_MIN_THRESHOLD &&
dataGap[1] <= ST_MAG40_ST_MAX_THRESHOLD &&
dataGap[2] >= ST_MAG40_ST_MIN_THRESHOLD &&
dataGap[2] <= ST_MAG40_ST_MAX_THRESHOLD)
sendTestResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_MAG);
else
sendTestResult(SENSOR_APP_EVT_STATUS_ERROR, SENS_TYPE_MAG);
mTask.mag_test_state = MAG_SELFTEST_DONE;
mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR, ST_MAG40_TEMP_COMP_EN | ST_MAG40_POWER_IDLE, 0, false);
mTask.comm_tx(ST_MAG40_CFG_C_REG_ADDR, ST_MAG40_BDU_ON | ST_MAG40_INT_MAG, 0, true);
break;
case MAG_SELFTEST_DONE:
break;
}
}
static bool magSelfTest(void *cookie)
{
INFO_PRINT("magSelfTest\n");
if (!mTask.magOn && trySwitchState(SENSOR_SELF_TEST)) {
mTask.mag_test_state = MAG_SELFTEST_INIT;
magTestHandling(NULL);
return true;
} else {
ERROR_PRINT("cannot test mag because sensor is busy\n");
sendTestResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_MAG);
return false;
}
}
#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_mag40_SensorOps =
{
DEC_OPS(magPower, magFwUpload, magSetRate, magFlush, magSelfTest, NULL, magCfgData),
};
static void enableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
{
gpioConfigInput(pin, GPIO_SPEED_LOW, GPIO_PULL_NONE);
syscfgSetExtiPort(pin);
extiEnableIntGpio(pin, EXTI_TRIGGER_RISING);
extiChainIsr(ST_MAG40_INT_IRQ, isr);
}
static void disableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
{
extiUnchainIsr(ST_MAG40_INT_IRQ, isr);
extiDisableIntGpio(pin);
}
static bool st_mag40_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_mag40_int1_isr: osEnqueuePrivateEvt() failed\n");
extiClearPendingGpio(mTask.Int1);
return true;
}
#define TIME_NS_TO_US(ns) cpuMathU64DivByU16(ns, 1000)
#define kScale_mag 0.15f /* in uT - (1.5f / 10) */
static void parseRawData(uint8_t *raw)
{
struct TripleAxisDataEvent *magSample;
int32_t raw_x = (*(int16_t *)&raw[0]);
int32_t raw_y = (*(int16_t *)&raw[2]);
int32_t raw_z = (*(int16_t *)&raw[4]);
float x, y, z;
float xs, ys, zs;
bool newMagnCalibData;
#if defined(ST_MAG40_CAL_ENABLED)
float xi, yi, zi;
#endif
mTask.timestampInt = sensorGetTime();
/* Discard samples generated during sensor turn-on time */
if (mTask.samplesToDiscard > 0) {
mTask.samplesToDiscard--;
return;
}
/* in uT */
xs = (float)raw_x * kScale_mag;
ys = (float)raw_y * kScale_mag;
zs = (float)raw_z * kScale_mag;
/* rotate axes */
x = ST_MAG40_REMAP_X_DATA(xs, ys, zs, ST_MAG40_ROT_MATRIX);
y = ST_MAG40_REMAP_Y_DATA(xs, ys, zs, ST_MAG40_ROT_MATRIX);
z = ST_MAG40_REMAP_Z_DATA(xs, ys, zs, ST_MAG40_ROT_MATRIX);
#if defined(ST_MAG40_CAL_ENABLED)
magCalRemoveSoftiron(&mTask.moc, x, y, z, &xi, &yi, &zi);
newMagnCalibData = magCalUpdate(&mTask.moc, TIME_NS_TO_US(mTask.timestampInt), xi, yi, zi);
magCalRemoveBias(&mTask.moc, xi, yi, zi, &x, &y, &z);
#endif
if (magAllocateEvt(&magSample) == false)
return;
magSample->referenceTime = mTask.timestampInt;
magSample->samples[0].deltaTime = 0;
magSample->samples[0].firstSample.numSamples = 1;
magSample->samples[0].x = x;
magSample->samples[0].y = y;
magSample->samples[0].z = z;
#if defined(ST_MAG40_CAL_ENABLED)
if (newMagnCalibData) {
magSample->samples[1].deltaTime = 0;
magCalGetBias(&mTask.moc,
&magSample->samples[1].x,
&magSample->samples[1].y,
&magSample->samples[1].z);
magSample->referenceTime = mTask.timestampInt;
magSample->samples[0].firstSample.numSamples = 2;
magSample->samples[0].firstSample.biasCurrent = true;
magSample->samples[0].firstSample.biasPresent = 1;
magSample->samples[0].firstSample.biasSample = 1;
}
#endif
osEnqueueEvtOrFree(sensorGetMyEventType(SENS_TYPE_MAG), magSample, magFreeEvt);
}
static uint8_t *wai;
static void int2Evt(void)
{
if (trySwitchState(SENSOR_READ_SAMPLES)) {
mTask.comm_rx(ST_MAG40_OUTXL_REG_ADDR, 6, 0, true);
} else {
mTask.pendingInt = true;
}
}
static void processPendingEvt(void)
{
if (mTask.pendingInt) {
mTask.pendingInt = false;
int2Evt();
return;
}
if (mTask.pendingSubState != NO_SUBSTATE) {
if (trySwitchState(SENSOR_MAG_CONFIGURATION)) {
mTask.subState = mTask.pendingSubState;
mTask.pendingSubState = NO_SUBSTATE;
sensorMagConfig();
}
}
}
static void sensorMagConfig(void)
{
uint8_t tmp;
switch (mTask.subState) {
case CONFIG_POWER_UP:
mTask.subState = CONFIG_POWER_UP_2;
mTask.comm_tx(ST_MAG40_CFG_B_REG_ADDR, ST_MAG40_OFF_CANC, 0, false);
mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR,
ST_MAG40_TEMP_COMP_EN | ST_MAG40_POWER_ON | mTask.currentODR, 0, true);
break;
case CONFIG_POWER_UP_2:
mTask.subState = CONFIG_DONE;
mTask.magOn = true;
sensorSignalInternalEvt(mTask.magHandle,
SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0);
break;
case CONFIG_POWER_DOWN:
mTask.subState = CONFIG_POWER_DOWN_2;
mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR,
ST_MAG40_TEMP_COMP_EN | ST_MAG40_POWER_IDLE | mTask.currentODR, 0, true);
break;
case CONFIG_POWER_DOWN_2:
mTask.subState = CONFIG_DONE;
mTask.magOn = false;
sensorSignalInternalEvt(mTask.magHandle,
SENSOR_INTERNAL_EVT_POWER_STATE_CHG, false, 0);
break;
case CONFIG_SET_RATE:
mTask.subState = CONFIG_SET_RATE_2;
tmp = mTask.magOn ? ST_MAG40_POWER_ON : ST_MAG40_POWER_IDLE;
tmp |= mTask.currentODR;
mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR, ST_MAG40_TEMP_COMP_EN | tmp, 0, true);
break;
case CONFIG_SET_RATE_2:
mTask.subState = CONFIG_DONE;
sensorSignalInternalEvt(mTask.magHandle,
SENSOR_INTERNAL_EVT_RATE_CHG, mTask.rate, mTask.latency);
break;
default:
/* Something weird happened */
ERROR_PRINT("sensorMagConfig() subState=%d\n", mTask.subState);
mTask.subState = CONFIG_DONE;
break;
}
}
/* initial sensor configuration */
static void sensorInit(void)
{
switch (mTask.subState) {
case INIT_START:
mTask.subState = INIT_ENABLE_DRDY;
mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR,
ST_MAG40_SOFT_RESET_BIT, 0, true);
break;
case INIT_ENABLE_DRDY:
mTask.subState = INIT_DONE;
mTask.comm_rx(ST_MAG40_OUTXL_REG_ADDR, 6, 0, false);
mTask.comm_tx(ST_MAG40_CFG_C_REG_ADDR,
ST_MAG40_BDU_ON | ST_MAG40_INT_MAG, 0, true);
break;
default:
/* Something weird happened */
ERROR_PRINT("sensorInit() subState=%d\n", mTask.subState);
mTask.subState = INIT_DONE;
break;
}
}
static void handleCommDoneEvt(const void* evtData)
{
bool returnIdle = false;
struct I2cTransfer *xfer = (struct I2cTransfer *)evtData;
switch (GET_STATE()) {
case SENSOR_BOOT:
SET_STATE(SENSOR_VERIFY_ID);
mTask.comm_rx(ST_MAG40_WAI_REG_ADDR, 1, 0, true);
break;
case SENSOR_VERIFY_ID:
/* Check the sensor ID */
wai = &xfer->txrxBuf[0];
if (ST_MAG40_WAI_REG_VAL != wai[0]) {
DEBUG_PRINT("WAI returned is: %02x\n\n", *wai);
SET_STATE(SENSOR_BOOT);
mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR,
ST_MAG40_SOFT_RESET_BIT, 0, true);
break;
}
INFO_PRINT( "Device ID is correct! (%02x)\n", *wai);
SET_STATE(SENSOR_INITIALIZATION);
mTask.subState = INIT_START;
sensorInit();
break;
case SENSOR_INITIALIZATION:
if (mTask.subState == INIT_DONE) {
INFO_PRINT( "Initialization completed\n");
returnIdle = true;
sensorRegisterInitComplete(mTask.magHandle);
} else {
sensorInit();
}
break;
case SENSOR_MAG_CONFIGURATION:
if (mTask.subState != CONFIG_DONE)
sensorMagConfig();
if (mTask.subState == CONFIG_DONE)
returnIdle = true;
break;
case SENSOR_READ_SAMPLES:
returnIdle = true;
if (gpioGet(mTask.Int1)) {
ERROR_PRINT("error read sensor, retry!\n");
}
if (mTask.magOn)
parseRawData(&xfer->txrxBuf[0]);
break;
case SENSOR_SELF_TEST:
if (mTask.mag_test_state == MAG_SELFTEST_DONE)
returnIdle = true;
else
magTestHandling(xfer);
break;
case SENSOR_IDLE:
default:
break;
}
releaseXfer(xfer);
if (returnIdle) {
SET_STATE(SENSOR_IDLE);
processPendingEvt();
}
}
static void 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_MAG40_CFG_A_REG_ADDR,
ST_MAG40_SOFT_RESET_BIT, 0, true);
break;
case EVT_COMM_DONE:
handleCommDoneEvt(evtData);
break;
case EVT_SENSOR_INTERRUPT:
int2Evt();
break;
default:
break;
}
}
static bool startTask(uint32_t task_id)
{
size_t slabSize;
mTask.tid = task_id;
INFO_PRINT("I2C DRIVER started\n");
mTask.magOn = false;
mTask.pendingInt = false;
mTask.pendingSubState = NO_SUBSTATE;
mTask.currentODR = ST_MAG40_ODR_10_HZ;
mTask.timestampInt = 0;
slabSize = sizeof(struct TripleAxisDataEvent) + sizeof(struct TripleAxisDataPoint);
#if defined(ST_MAG40_CAL_ENABLED)
slabSize += sizeof(struct TripleAxisDataPoint);
#endif
mTask.magDataSlab = slabAllocatorNew(slabSize, 4, ST_MAG40_MAX_MAG_EVENTS);
if (!mTask.magDataSlab) {
ERROR_PRINT("Failed to allocate magDataSlab memory\n");
return false;
}
/* Init the communication part */
i2cMasterRequest(ST_MAG40_I2C_BUS_ID, ST_MAG40_I2C_SPEED);
mTask.comm_tx = i2c_write;
mTask.comm_rx = i2c_read;
/* irq */
mTask.Int1 = gpioRequest(ST_MAG40_INT_PIN);
gpioConfigInput(mTask.Int1, GPIO_SPEED_LOW, GPIO_PULL_NONE);
mTask.Isr1.func = st_mag40_int1_isr;
enableInterrupt(mTask.Int1, &mTask.Isr1);
#if defined(ST_MAG40_CAL_ENABLED)
#ifdef DIVERSITY_CHECK_ENABLED
initMagCal(&mTask.moc,
0.0f, 0.0f, 0.0f, // bias x, y, z
1.0f, 0.0f, 0.0f, // c00, c01, c02
0.0f, 1.0f, 0.0f, // c10, c11, c12
0.0f, 0.0f, 1.0f, // c20, c21, c22
3000000, // min_batch_window_in_micros
8, // min_num_diverse_vectors
1, // max_num_max_distance
6.0f, // var_threshold
10.0f, // max_min_threshold
48.f, // local_field
0.5f, // threshold_tuning_param
2.552f); // max_distance_tuning_param
#else
initMagCal(&mTask.moc,
0.0f, 0.0f, 0.0f, // bias x, y, z
1.0f, 0.0f, 0.0f, // c00, c01, c02
0.0f, 1.0f, 0.0f, // c10, c11, c12
0.0f, 0.0f, 1.0f, // c20, c21, c22
3000000); // min_batch_window_in_micros
#endif /* DIVERSITY_CHECK_ENABLED */
#endif /* ST_MAG40_CAL_ENABLED */
mTask.magHandle =
sensorRegister(&st_mag40_SensorInfo, &st_mag40_SensorOps, NULL, false);
osEventSubscribe(mTask.tid, EVT_APP_START);
return true;
}
static void endTask(void)
{
INFO_PRINT("ended\n");
#if defined(ST_MAG40_CAL_ENABLED)
magCalDestroy(&mTask.moc);
#endif /* ST_MAG40_CAL_ENABLED */
slabAllocatorDestroy(mTask.magDataSlab);
disableInterrupt(mTask.Int1, &mTask.Isr1);
}
INTERNAL_APP_INIT(ST_MAG40_APP_ID, 0, startTask, endTask, handleEvent);