blob: e5b0e77228f0fd3cdc88dbf0ac84a68937a6942e [file] [log] [blame]
/*
* 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 <algos/time_sync.h>
#include <atomic.h>
#include <cpu/inc/cpuMath.h>
#include <gpio.h>
#include <heap.h>
#include <hostIntf.h>
#include <isr.h>
#include <nanohub_math.h>
#include <nanohubPacket.h>
#include <plat/inc/exti.h>
#include <plat/inc/gpio.h>
#include <plat/inc/syscfg.h>
#include <plat/inc/rtc.h>
#include <sensors.h>
#include <seos.h>
#include <slab.h>
#include <spi.h>
#include <timer.h>
#include <variant/inc/sensType.h>
#include <variant/inc/variant.h>
#ifdef MAG_SLAVE_PRESENT
#include <algos/mag_cal.h>
#endif
#ifdef ACCEL_CAL_ENABLED
#include <algos/accel_cal.h>
#endif
#ifdef GYRO_CAL_ENABLED
// Gyro Cal -- Header.
#include <algos/gyro_cal.h>
#endif
#include <limits.h>
#include <stdlib.h>
#include <string.h>
#define INFO_PRINT(fmt, ...) do { \
osLog(LOG_INFO, "%s " fmt, "[BMI160]", ##__VA_ARGS__); \
} while (0);
#define ERROR_PRINT(fmt, ...) do { \
osLog(LOG_ERROR, "%s " fmt, "[BMI160] ERROR:", ##__VA_ARGS__); \
} while (0);
#define DEBUG_PRINT(fmt, ...) do { \
if (DBG_ENABLE) { \
INFO_PRINT(fmt, ##__VA_ARGS__); \
} \
} while (0);
#define DEBUG_PRINT_IF(cond, fmt, ...) do { \
if ((cond) && DBG_ENABLE) { \
INFO_PRINT(fmt, ##__VA_ARGS__); \
} \
} while (0);
#define DBG_ENABLE 0
#define DBG_CHUNKED 0
#define DBG_INT 0
#define DBG_SHALLOW_PARSE 0
#define DBG_STATE 0
#define DBG_WM_CALC 0
#define TIMESTAMP_DBG 0
#define BMI160_APP_VERSION 6
// fixme: to list required definitions for a slave mag
#ifdef USE_BMM150
#include "bosch_bmm150_slave.h"
#elif USE_AK09915
#include "akm_ak09915_slave.h"
#endif
#define BMI160_APP_ID APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 2)
#define BMI160_SPI_WRITE 0x00
#define BMI160_SPI_READ 0x80
#define BMI160_SPI_BUS_ID 1
#define BMI160_SPI_SPEED_HZ 8000000
#define BMI160_SPI_MODE 3
#define BMI160_INT_IRQ EXTI9_5_IRQn
#define BMI160_INT1_PIN GPIO_PB(6)
#define BMI160_INT2_PIN GPIO_PB(7)
#define BMI160_ID 0xd1
#define BMI160_REG_ID 0x00
#define BMI160_REG_ERR 0x02
#define BMI160_REG_PMU_STATUS 0x03
#define BMI160_REG_DATA_0 0x04
#define BMI160_REG_DATA_1 0x05
#define BMI160_REG_DATA_14 0x12
#define BMI160_REG_SENSORTIME_0 0x18
#define BMI160_REG_STATUS 0x1b
#define BMI160_REG_INT_STATUS_0 0x1c
#define BMI160_REG_INT_STATUS_1 0x1d
#define BMI160_REG_TEMPERATURE_0 0x20
#define BMI160_REG_TEMPERATURE_1 0x21
#define BMI160_REG_FIFO_LENGTH_0 0x22
#define BMI160_REG_FIFO_DATA 0x24
#define BMI160_REG_ACC_CONF 0x40
#define BMI160_REG_ACC_RANGE 0x41
#define BMI160_REG_GYR_CONF 0x42
#define BMI160_REG_GYR_RANGE 0x43
#define BMI160_REG_MAG_CONF 0x44
#define BMI160_REG_FIFO_DOWNS 0x45
#define BMI160_REG_FIFO_CONFIG_0 0x46
#define BMI160_REG_FIFO_CONFIG_1 0x47
#define BMI160_REG_MAG_IF_0 0x4b
#define BMI160_REG_MAG_IF_1 0x4c
#define BMI160_REG_MAG_IF_2 0x4d
#define BMI160_REG_MAG_IF_3 0x4e
#define BMI160_REG_MAG_IF_4 0x4f
#define BMI160_REG_INT_EN_0 0x50
#define BMI160_REG_INT_EN_1 0x51
#define BMI160_REG_INT_EN_2 0x52
#define BMI160_REG_INT_OUT_CTRL 0x53
#define BMI160_REG_INT_LATCH 0x54
#define BMI160_REG_INT_MAP_0 0x55
#define BMI160_REG_INT_MAP_1 0x56
#define BMI160_REG_INT_MAP_2 0x57
#define BMI160_REG_INT_DATA_0 0x58
#define BMI160_REG_INT_MOTION_0 0x5f
#define BMI160_REG_INT_MOTION_1 0x60
#define BMI160_REG_INT_MOTION_2 0x61
#define BMI160_REG_INT_MOTION_3 0x62
#define BMI160_REG_INT_TAP_0 0x63
#define BMI160_REG_INT_TAP_1 0x64
#define BMI160_REG_INT_FLAT_0 0x67
#define BMI160_REG_INT_FLAT_1 0x68
#define BMI160_REG_PMU_TRIGGER 0x6C
#define BMI160_REG_FOC_CONF 0x69
#define BMI160_REG_CONF 0x6a
#define BMI160_REG_IF_CONF 0x6b
#define BMI160_REG_SELF_TEST 0x6d
#define BMI160_REG_OFFSET_0 0x71
#define BMI160_REG_OFFSET_3 0x74
#define BMI160_REG_OFFSET_6 0x77
#define BMI160_REG_STEP_CNT_0 0x78
#define BMI160_REG_STEP_CONF_0 0x7a
#define BMI160_REG_STEP_CONF_1 0x7b
#define BMI160_REG_CMD 0x7e
#define BMI160_REG_MAGIC 0x7f
#define INT_STEP 0x01
#define INT_ANY_MOTION 0x04
#define INT_DOUBLE_TAP 0x10
#define INT_SINGLE_TAP 0x20
#define INT_ORIENT 0x40
#define INT_FLAT 0x80
#define INT_HIGH_G_Z 0x04
#define INT_LOW_G 0x08
#define INT_DATA_RDY 0x10
#define INT_FIFO_FULL 0x20
#define INT_FIFO_WM 0x40
#define INT_NO_MOTION 0x80
#define BMI160_FRAME_HEADER_INVALID 0x80 // mark the end of valid data
#define BMI160_FRAME_HEADER_SKIP 0x81 // not defined by hw, used for skip a byte in buffer
#define WATERMARK_MIN 1
#define WATERMARK_MAX 200 // must <= 255 (0xff)
#define WATERMARK_MAX_SENSOR_RATE 400 // Accel and gyro are 400 Hz max
#define WATERMARK_TIME_UNIT_NS (1000000000ULL/(WATERMARK_MAX_SENSOR_RATE))
#define gSPI BMI160_SPI_BUS_ID
#define ACCL_INT_LINE EXTI_LINE_P6
#define GYR_INT_LINE EXTI_LINE_P7
#define SPI_WRITE_0(addr, data) spiQueueWrite(addr, data, 2)
#define SPI_WRITE_1(addr, data, delay) spiQueueWrite(addr, data, delay)
#define GET_SPI_WRITE_MACRO(_1,_2,_3,NAME,...) NAME
#define SPI_WRITE(...) GET_SPI_WRITE_MACRO(__VA_ARGS__, SPI_WRITE_1, SPI_WRITE_0)(__VA_ARGS__)
#define SPI_READ_0(addr, size, buf) spiQueueRead(addr, size, buf, 0)
#define SPI_READ_1(addr, size, buf, delay) spiQueueRead(addr, size, buf, delay)
#define GET_SPI_READ_MACRO(_1,_2,_3,_4,NAME,...) NAME
#define SPI_READ(...) GET_SPI_READ_MACRO(__VA_ARGS__, SPI_READ_1, SPI_READ_0)(__VA_ARGS__)
#define EVT_SENSOR_ACC_DATA_RDY sensorGetMyEventType(SENS_TYPE_ACCEL)
#define EVT_SENSOR_GYR_DATA_RDY sensorGetMyEventType(SENS_TYPE_GYRO)
#define EVT_SENSOR_MAG_DATA_RDY sensorGetMyEventType(SENS_TYPE_MAG)
#define EVT_SENSOR_STEP sensorGetMyEventType(SENS_TYPE_STEP_DETECT)
#define EVT_SENSOR_NO_MOTION sensorGetMyEventType(SENS_TYPE_NO_MOTION)
#define EVT_SENSOR_ANY_MOTION sensorGetMyEventType(SENS_TYPE_ANY_MOTION)
#define EVT_SENSOR_FLAT sensorGetMyEventType(SENS_TYPE_FLAT)
#define EVT_SENSOR_DOUBLE_TAP sensorGetMyEventType(SENS_TYPE_DOUBLE_TAP)
#define EVT_SENSOR_STEP_COUNTER sensorGetMyEventType(SENS_TYPE_STEP_COUNT)
#define MAX_NUM_COMMS_EVENT_SAMPLES 15
#define kScale_acc 0.00239501953f // ACC_range * 9.81f / 32768.0f;
#define kScale_gyr 0.00053263221f // GYR_range * M_PI / (180.0f * 32768.0f);
#define kScale_temp 0.001953125f // temperature in deg C
#define kTempInvalid -1000.0f
#define kTimeSyncPeriodNs 100000000ull // sync sensor and RTC time every 100ms
#define kSensorTimerIntervalUs 39ull // bmi160 clock increaments every 39000ns
#define kMinRTCTimeIncrementNs 1250000ull // forced min rtc time increment, 1.25ms for 400Hz
#define kMinSensorTimeIncrement 64 // forced min sensortime increment,
// 64 = 2.5 msec for 400Hz
#define ACC_MIN_RATE 5
#define GYR_MIN_RATE 6
#define ACC_MAX_RATE 12
#define GYR_MAX_RATE 13
#define MAG_MAX_RATE 11
#define ACC_MAX_OSR 3
#define GYR_MAX_OSR 4
#define OSR_THRESHOLD 8
#define MOTION_ODR 7
#define RETRY_CNT_CALIBRATION 10
#define RETRY_CNT_ID 5
#define RETRY_CNT_MAG 30
#define SPI_PACKET_SIZE 30
#define FIFO_READ_SIZE (1024+4)
#define CHUNKED_READ_SIZE (64)
#define BUF_MARGIN 32 // some extra buffer for additional reg RW when a FIFO read happens
#define SPI_BUF_SIZE (FIFO_READ_SIZE + CHUNKED_READ_SIZE + BUF_MARGIN)
#define ABS(x) (((x) > 0) ? (x) : -(x))
enum SensorIndex {
FIRST_CONT_SENSOR = 0,
ACC = FIRST_CONT_SENSOR,
GYR,
#ifdef MAG_SLAVE_PRESENT
MAG,
#endif
NUM_CONT_SENSOR,
FIRST_ONESHOT_SENSOR = NUM_CONT_SENSOR,
STEP = FIRST_ONESHOT_SENSOR,
DTAP,
FLAT,
ANYMO,
NOMO,
STEPCNT,
NUM_OF_SENSOR,
};
enum SensorEvents {
NO_EVT = -1,
EVT_SPI_DONE = EVT_APP_START + 1,
EVT_SENSOR_INTERRUPT_1,
EVT_SENSOR_INTERRUPT_2,
EVT_TIME_SYNC,
};
enum InitState {
RESET_BMI160,
INIT_BMI160,
INIT_MAG,
INIT_ON_CHANGE_SENSORS,
INIT_DONE,
};
enum CalibrationState {
CALIBRATION_START,
CALIBRATION_FOC,
CALIBRATION_WAIT_FOC_DONE,
CALIBRATION_SET_OFFSET,
CALIBRATION_DONE,
CALIBRATION_TIMEOUT,
};
enum AccTestState {
ACC_TEST_START,
ACC_TEST_CONFIG,
ACC_TEST_RUN_0,
ACC_TEST_RUN_1,
ACC_TEST_VERIFY,
ACC_TEST_DONE
};
enum GyroTestState {
GYRO_TEST_START,
GYRO_TEST_RUN,
GYRO_TEST_VERIFY,
GYRO_TEST_DONE
};
enum SensorState {
// keep this in sync with getStateName
SENSOR_BOOT,
SENSOR_VERIFY_ID,
SENSOR_INITIALIZING,
SENSOR_IDLE,
SENSOR_POWERING_UP,
SENSOR_POWERING_DOWN,
SENSOR_CONFIG_CHANGING,
SENSOR_INT_1_HANDLING,
SENSOR_INT_2_HANDLING,
SENSOR_CALIBRATING,
SENSOR_TESTING,
SENSOR_STEP_CNT,
SENSOR_TIME_SYNC,
SENSOR_SAVE_CALIBRATION,
SENSOR_NUM_OF_STATE
};
#if DBG_STATE
#define PRI_STATE "s"
static const char * getStateName(int32_t s) {
// keep this in sync with SensorState
static const char* const l[] = {"BOOT", "VERIFY_ID", "INIT", "IDLE", "PWR_UP",
"PWR-DN", "CFG_CHANGE", "INT1", "INT2", "CALIB", "STEP_CNT", "SYNC", "SAVE_CALIB"};
if (s >= 0 && s < SENSOR_NUM_OF_STATE) {
return l[s];
}
return "???";
#else
#define PRI_STATE PRIi32
static int32_t getStateName(int32_t s) {
return s;
#endif
}
enum MagConfigState {
MAG_SET_START,
MAG_SET_IF,
// BMM150 only
MAG_SET_REPXY,
MAG_SET_REPZ,
MAG_GET_DIG_X,
MAG_GET_DIG_Y,
MAG_GET_DIG_Z,
MAG_SET_SAVE_DIG,
MAG_SET_FORCE,
MAG_SET_ADDR,
MAG_SET_DATA,
MAG_SET_DONE,
MAG_INIT_FAILED
};
struct ConfigStat {
uint64_t latency;
uint32_t rate;
bool enable;
};
struct CalibrationData {
struct HostHubRawPacket header;
struct SensorAppEventHeader data_header;
int32_t xBias;
int32_t yBias;
int32_t zBias;
} __attribute__((packed));
struct TestResultData {
struct HostHubRawPacket header;
struct SensorAppEventHeader data_header;
} __attribute__((packed));
struct BMI160Sensor {
struct ConfigStat pConfig; // pending config status request
struct TripleAxisDataEvent *data_evt;
uint32_t handle;
uint32_t rate;
uint64_t latency;
uint64_t prev_rtc_time;
uint32_t offset[3];
bool powered; // activate status
bool configed; // configure status
bool offset_enable;
uint8_t flush;
enum SensorIndex idx;
};
struct BMI160Task {
uint32_t tid;
struct BMI160Sensor sensors[NUM_OF_SENSOR];
#ifdef GYRO_CAL_ENABLED
// Gyro Cal -- Declaration.
struct gyroCal_t gyro_cal;
#ifdef GYRO_CAL_DBG_ENABLED
// Gyro Cal -- Read out Debug data.
int gyro_debug_state;
#endif
#endif
// time keeping.
uint64_t last_sensortime;
uint64_t frame_sensortime;
uint64_t prev_frame_time[NUM_CONT_SENSOR];
uint64_t time_delta[NUM_CONT_SENSOR];
uint64_t next_delta[NUM_CONT_SENSOR];
uint64_t tempTime;
// spi and interrupt
spi_cs_t cs;
struct SpiMode mode;
struct SpiPacket packets[SPI_PACKET_SIZE];
struct SpiDevice *spiDev;
struct Gpio *Int1;
struct Gpio *Int2;
struct ChainedIsr Isr1;
struct ChainedIsr Isr2;
#ifdef ACCEL_CAL_ENABLED
struct accelCal_t acc;
#endif
#ifdef MAG_SLAVE_PRESENT
struct MagCal moc;
#endif
time_sync_t gSensorTime2RTC;
float tempCelsius;
float last_charging_bias_x;
uint32_t total_step_cnt;
uint32_t last_step_cnt;
uint32_t poll_generation;
uint32_t active_poll_generation;
uint8_t active_oneshot_sensor_cnt;
uint8_t interrupt_enable_0;
uint8_t interrupt_enable_2;
uint8_t acc_downsample;
uint8_t gyr_downsample;
bool magBiasPosted;
bool magBiasCurrent;
bool fifo_enabled[NUM_CONT_SENSOR];
// for step count
uint32_t stepCntSamplingTimerHandle;
bool step_cnt_changed;
// spi buffers
int xferCnt;
uint8_t *dataBuffer;
uint8_t *statusBuffer;
uint8_t *sensorTimeBuffer;
uint8_t *temperatureBuffer;
uint8_t txrxBuffer[SPI_BUF_SIZE];
// states
volatile uint8_t state; //task state, type enum SensorState, do NOT change this directly
enum InitState init_state;
enum MagConfigState mag_state;
enum CalibrationState calibration_state;
enum AccTestState acc_test_state;
enum GyroTestState gyro_test_state;
// for self-test
int16_t accTestX, accTestY, accTestZ;
// pending configs
bool pending_int[2];
bool pending_step_cnt;
bool pending_config[NUM_OF_SENSOR];
bool pending_calibration_save;
bool pending_time_sync;
bool pending_delta[NUM_CONT_SENSOR];
bool pending_dispatch;
bool frame_sensortime_valid;
// FIFO setting
uint16_t chunkReadSize;
uint8_t watermark;
// spi rw
struct SlabAllocator *mDataSlab;
uint16_t mWbufCnt;
uint8_t mRegCnt;
uint8_t mRetryLeft;
bool spiInUse;
};
static uint32_t AccRates[] = {
SENSOR_HZ(25.0f/8.0f),
SENSOR_HZ(25.0f/4.0f),
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),
0,
};
static uint32_t GyrRates[] = {
SENSOR_HZ(25.0f/8.0f),
SENSOR_HZ(25.0f/4.0f),
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),
0,
};
#ifdef MAG_SLAVE_PRESENT
static uint32_t MagRates[] = {
SENSOR_HZ(25.0f/8.0f),
SENSOR_HZ(25.0f/4.0f),
SENSOR_HZ(25.0f/2.0f),
SENSOR_HZ(25.0f),
SENSOR_HZ(50.0f),
SENSOR_HZ(100.0f),
0,
};
#endif
static uint32_t StepCntRates[] = {
SENSOR_HZ(1.0f/300.0f),
SENSOR_HZ(1.0f/240.0f),
SENSOR_HZ(1.0f/180.0f),
SENSOR_HZ(1.0f/120.0f),
SENSOR_HZ(1.0f/90.0f),
SENSOR_HZ(1.0f/60.0f),
SENSOR_HZ(1.0f/45.0f),
SENSOR_HZ(1.0f/30.0f),
SENSOR_HZ(1.0f/15.0f),
SENSOR_HZ(1.0f/10.0f),
SENSOR_HZ(1.0f/5.0f),
SENSOR_RATE_ONCHANGE,
0
};
static const uint64_t stepCntRateTimerVals[] = // should match StepCntRates and be the timer length for that rate in nanosecs
{
300 * 1000000000ULL,
240 * 1000000000ULL,
180 * 1000000000ULL,
120 * 1000000000ULL,
90 * 1000000000ULL,
60 * 1000000000ULL,
45 * 1000000000ULL,
30 * 1000000000ULL,
15 * 1000000000ULL,
10 * 1000000000ULL,
5 * 1000000000ULL,
};
static struct BMI160Task mTask;
#ifdef MAG_SLAVE_PRESENT
static struct MagTask magTask;
#endif
#define MAG_WRITE(addr, data) \
do { \
SPI_WRITE(BMI160_REG_MAG_IF_4, data); \
SPI_WRITE(BMI160_REG_MAG_IF_3, addr); \
} while (0)
#define MAG_READ(addr, size) \
do { \
SPI_WRITE(BMI160_REG_MAG_IF_2, addr, 5000); \
SPI_READ(BMI160_REG_DATA_0, size, &mTask.dataBuffer); \
} while (0)
#define DEC_INFO(name, type, axis, inter, samples) \
.sensorName = name, \
.sensorType = type, \
.numAxis = axis, \
.interrupt = inter, \
.minSamples = samples
#define DEC_INFO_RATE(name, rates, type, axis, inter, samples) \
DEC_INFO(name, type, axis, inter, samples), \
.supportedRates = rates
#define DEC_INFO_RATE_RAW(name, rates, type, axis, inter, samples, raw, scale) \
DEC_INFO(name, type, axis, inter, samples), \
.supportedRates = rates, \
.flags1 = SENSOR_INFO_FLAGS1_RAW, \
.rawType = raw, \
.rawScale = scale
#define DEC_INFO_RATE_BIAS(name, rates, type, axis, inter, samples, bias) \
DEC_INFO(name, type, axis, inter, samples), \
.supportedRates = rates, \
.flags1 = SENSOR_INFO_FLAGS1_BIAS, \
.biasType = bias
typedef struct BMI160Task _Task;
#define TASK _Task* const _task
// To get rid of static variables all task functions should have a task structure pointer input.
// This is an intermediate step.
#define TDECL() TASK = &mTask; (void)_task
// Access task variables without explicitly specify the task structure pointer.
#define T(v) (_task->v)
// Atomic get state
#define GET_STATE() (atomicReadByte(&(_task->state)))
// Atomic set state, this set the state to arbitrary value, use with caution
#define SET_STATE(s) do{\
DEBUG_PRINT_IF(DBG_STATE, "set state %" PRI_STATE "\n", getStateName(s));\
atomicWriteByte(&(_task->state), (s));\
}while(0)
// Atomic switch state from IDLE to desired state.
static bool trySwitchState_(TASK, enum SensorState newState) {
#if DBG_STATE
bool ret = atomicCmpXchgByte(&T(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(&T(state), SENSOR_IDLE, newState);
#endif
}
// Short-hand
#define trySwitchState(s) trySwitchState_(_task, (s))
// Chunked FIFO read functions
static void chunkedReadInit_(TASK, int index, int size);
#define chunkedReadInit(a,b) chunkedReadInit_(_task, (a), (b))
static void chunkedReadSpiCallback(void *cookie, int error);
static void initiateFifoRead_(TASK, bool isInterruptContext);
#define initiateFifoRead(a) initiateFifoRead_(_task, (a))
static uint8_t* shallowParseFrame(uint8_t * buf, int size);
// Watermark calculation
static uint8_t calcWatermark2_(TASK);
#define calcWatermark2() calcWatermark2_(_task)
static const struct SensorInfo mSensorInfo[NUM_OF_SENSOR] =
{
{ DEC_INFO_RATE_RAW("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE,
NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0/kScale_acc) },
{ DEC_INFO_RATE_BIAS("Gyroscope", GyrRates, SENS_TYPE_GYRO, NUM_AXIS_THREE,
NANOHUB_INT_NONWAKEUP, 20, SENS_TYPE_GYRO_BIAS) },
#ifdef MAG_SLAVE_PRESENT
{ DEC_INFO_RATE_BIAS("Magnetometer", MagRates, SENS_TYPE_MAG, NUM_AXIS_THREE,
NANOHUB_INT_NONWAKEUP, 600, SENS_TYPE_MAG_BIAS) },
#endif
{ DEC_INFO("Step Detector", SENS_TYPE_STEP_DETECT, NUM_AXIS_EMBEDDED,
NANOHUB_INT_NONWAKEUP, 100) },
{ DEC_INFO("Double Tap", SENS_TYPE_DOUBLE_TAP, NUM_AXIS_EMBEDDED,
NANOHUB_INT_NONWAKEUP, 20) },
{ DEC_INFO("Flat", SENS_TYPE_FLAT, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) },
{ DEC_INFO("Any Motion", SENS_TYPE_ANY_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) },
{ DEC_INFO("No Motion", SENS_TYPE_NO_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) },
{ DEC_INFO_RATE("Step Counter", StepCntRates, SENS_TYPE_STEP_COUNT, NUM_AXIS_EMBEDDED,
NANOHUB_INT_NONWAKEUP, 20) },
};
static void time_init(void) {
time_sync_init(&mTask.gSensorTime2RTC);
}
static bool sensortime_to_rtc_time(uint64_t sensor_time, uint64_t *rtc_time_ns) {
// fixme: nsec?
return time_sync_estimate_time1(
&mTask.gSensorTime2RTC, sensor_time * 39ull, rtc_time_ns);
}
static void map_sensortime_to_rtc_time(uint64_t sensor_time, uint64_t rtc_time_ns) {
// fixme: nsec?
time_sync_add(&mTask.gSensorTime2RTC, rtc_time_ns, sensor_time * 39ull);
}
static void invalidate_sensortime_to_rtc_time(void) {
time_sync_reset(&mTask.gSensorTime2RTC);
}
static void minimize_sensortime_history(void) {
// truncate datapoints to the latest two to maintain valid sensortime to rtc
// mapping and minimize the inflence of the past mapping
time_sync_truncate(&mTask.gSensorTime2RTC, 2);
// drop the oldest datapoint when a new one arrives for two times to
// completely shift out the influence of the past mapping
time_sync_hold(&mTask.gSensorTime2RTC, 2);
}
static void dataEvtFree(void *ptr)
{
TDECL();
struct TripleAxisDataEvent *ev = (struct TripleAxisDataEvent *)ptr;
slabAllocatorFree(T(mDataSlab), ev);
}
static void spiQueueWrite(uint8_t addr, uint8_t data, uint32_t delay)
{
TDECL();
if (T(spiInUse)) {
ERROR_PRINT("SPI in use, cannot queue write\n");
return;
}
T(packets[T(mRegCnt)]).size = 2;
T(packets[T(mRegCnt)]).txBuf = &T(txrxBuffer[T(mWbufCnt)]);
T(packets[T(mRegCnt)]).rxBuf = &T(txrxBuffer[T(mWbufCnt)]);
T(packets[T(mRegCnt)]).delay = delay * 1000;
T(txrxBuffer[T(mWbufCnt++)]) = BMI160_SPI_WRITE | addr;
T(txrxBuffer[T(mWbufCnt++)]) = data;
T(mRegCnt)++;
}
/*
* need to be sure size of buf is larger than read size
*/
static void spiQueueRead(uint8_t addr, size_t size, uint8_t **buf, uint32_t delay)
{
TDECL();
if (T(spiInUse)) {
ERROR_PRINT("SPI in use, cannot queue read %d %d\n", (int)addr, (int)size);
return;
}
*buf = &T(txrxBuffer[T(mWbufCnt)]);
T(packets[T(mRegCnt)]).size = size + 1; // first byte will not contain valid data
T(packets[T(mRegCnt)]).txBuf = &T(txrxBuffer[T(mWbufCnt)]);
T(packets[T(mRegCnt)]).rxBuf = *buf;
T(packets[T(mRegCnt)]).delay = delay * 1000;
T(txrxBuffer[T(mWbufCnt)++]) = BMI160_SPI_READ | addr;
T(mWbufCnt) += size;
T(mRegCnt)++;
}
static void spiBatchTxRx(struct SpiMode *mode,
SpiCbkF callback, void *cookie, const char * src)
{
TDECL();
if (T(mWbufCnt) > SPI_BUF_SIZE) {
ERROR_PRINT("NO enough SPI buffer space, dropping transaction.\n");
return;
}
if (T(mRegCnt) > SPI_PACKET_SIZE) {
ERROR_PRINT("spiBatchTxRx too many packets!\n");
return;
}
T(spiInUse) = true;
spiMasterRxTx(T(spiDev), T(cs), T(packets), T(mRegCnt), mode, callback, cookie);
T(mRegCnt) = 0;
T(mWbufCnt) = 0;
}
static bool bmi160Isr1(struct ChainedIsr *isr)
{
TASK = container_of(isr, struct BMI160Task, Isr1);
if (!extiIsPendingGpio(T(Int1))) {
return false;
}
DEBUG_PRINT_IF(DBG_INT, "i1\n");
initiateFifoRead(true /*isInterruptContext*/);
extiClearPendingGpio(T(Int1));
return true;
}
static bool bmi160Isr2(struct ChainedIsr *isr)
{
TASK = container_of(isr, struct BMI160Task, Isr2);
if (!extiIsPendingGpio(T(Int2)))
return false;
DEBUG_PRINT_IF(DBG_INT, "i2\n");
osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT_2, _task, NULL, T(tid));
extiClearPendingGpio(T(Int2));
return true;
}
static void sensorSpiCallback(void *cookie, int err)
{
mTask.spiInUse = false;
osEnqueuePrivateEvt(EVT_SPI_DONE, cookie, NULL, mTask.tid);
}
static void sensorTimerCallback(uint32_t timerId, void *data)
{
osEnqueuePrivateEvt(EVT_SPI_DONE, data, NULL, mTask.tid);
}
static void timeSyncCallback(uint32_t timerId, void *data)
{
osEnqueuePrivateEvt(EVT_TIME_SYNC, data, NULL, mTask.tid);
}
static void stepCntSamplingCallback(uint32_t timerId, void *data)
{
union EmbeddedDataPoint step_cnt;
if (mTask.sensors[STEPCNT].powered && mTask.step_cnt_changed) {
mTask.step_cnt_changed = false;
step_cnt.idata = mTask.total_step_cnt;
osEnqueueEvt(EVT_SENSOR_STEP_COUNTER, step_cnt.vptr, NULL);
}
}
static bool accFirmwareUpload(void *cookie)
{
sensorSignalInternalEvt(mTask.sensors[ACC].handle,
SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
return true;
}
static bool gyrFirmwareUpload(void *cookie)
{
sensorSignalInternalEvt(mTask.sensors[GYR].handle,
SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
return true;
}
#ifdef MAG_SLAVE_PRESENT
static bool magFirmwareUpload(void *cookie)
{
sensorSignalInternalEvt(mTask.sensors[MAG].handle,
SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
return true;
}
#endif
static bool stepFirmwareUpload(void *cookie)
{
sensorSignalInternalEvt(mTask.sensors[STEP].handle,
SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
return true;
}
static bool doubleTapFirmwareUpload(void *cookie)
{
sensorSignalInternalEvt(mTask.sensors[DTAP].handle,
SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
return true;
}
static bool noMotionFirmwareUpload(void *cookie)
{
sensorSignalInternalEvt(mTask.sensors[NOMO].handle,
SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
return true;
}
static bool anyMotionFirmwareUpload(void *cookie)
{
sensorSignalInternalEvt(mTask.sensors[ANYMO].handle,
SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
return true;
}
static bool flatFirmwareUpload(void *cookie)
{
sensorSignalInternalEvt(mTask.sensors[FLAT].handle,
SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
return true;
}
static bool stepCntFirmwareUpload(void *cookie)
{
sensorSignalInternalEvt(mTask.sensors[STEPCNT].handle,
SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
return true;
}
static bool enableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
{
gpioConfigInput(pin, GPIO_SPEED_LOW, GPIO_PULL_NONE);
syscfgSetExtiPort(pin);
extiEnableIntGpio(pin, EXTI_TRIGGER_RISING);
extiChainIsr(BMI160_INT_IRQ, isr);
return true;
}
static bool disableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
{
extiUnchainIsr(BMI160_INT_IRQ, isr);
extiDisableIntGpio(pin);
return true;
}
static void magConfigMagic(void)
{
// set the MAG power to NORMAL mode
SPI_WRITE(BMI160_REG_CMD, 0x19, 10000);
// Magic register sequence to shift register page table to access hidden
// register
SPI_WRITE(BMI160_REG_CMD, 0x37);
SPI_WRITE(BMI160_REG_CMD, 0x9a);
SPI_WRITE(BMI160_REG_CMD, 0xc0);
SPI_WRITE(BMI160_REG_MAGIC, 0x90);
SPI_READ(BMI160_REG_DATA_1, 1, &mTask.dataBuffer);
}
static void magConfigIf(void)
{
// Set the on-chip I2C pull-up register settings and shift the register
// table back down (magic)
SPI_WRITE(BMI160_REG_DATA_1, mTask.dataBuffer[1] | 0x30);
SPI_WRITE(BMI160_REG_MAGIC, 0x80);
// Config the MAG I2C device address
#ifdef MAG_SLAVE_PRESENT
SPI_WRITE(BMI160_REG_MAG_IF_0, (MAG_I2C_ADDR << 1));
#endif
// set mag_manual_enable, mag_offset=0, mag_rd_burst='8 bytes'
SPI_WRITE(BMI160_REG_MAG_IF_1, 0x83);
// primary interface: autoconfig, secondary: magnetometer.
SPI_WRITE(BMI160_REG_IF_CONF, 0x20);
// fixme: move to mag-specific function
#ifdef USE_BMM150
// set mag to SLEEP mode
MAG_WRITE(BMM150_REG_CTRL_1, 0x01);
#elif USE_AK09915
// set "low" Noise Suppression Filter (NSF) settings
MAG_WRITE(AKM_AK09915_REG_CNTL1, 0x20);
#endif
}
// fixme: break this up to master/slave-specific, so it'll be eventually slave-agnostic,
// and slave provides its own stateless config function
// fixme: not all async_elem_t is supported
static void magConfig(void)
{
switch (mTask.mag_state) {
case MAG_SET_START:
magConfigMagic();
mTask.mag_state = MAG_SET_IF;
break;
case MAG_SET_IF:
magConfigIf();
#ifdef USE_AK09915
mTask.mag_state = MAG_SET_FORCE;
#elif USE_BMM150
mTask.mag_state = MAG_SET_REPXY;
#endif
break;
#ifdef USE_BMM150
case MAG_SET_REPXY:
// MAG_SET_REPXY and MAG_SET_REPZ case set:
// regular preset, f_max,ODR ~ 102 Hz
MAG_WRITE(BMM150_REG_REPXY, 9);
mTask.mag_state = MAG_SET_REPZ;
break;
case MAG_SET_REPZ:
MAG_WRITE(BMM150_REG_REPZ, 15);
mTask.mag_state = MAG_GET_DIG_X;
break;
case MAG_GET_DIG_X:
// MAG_GET_DIG_X, MAG_GET_DIG_Y and MAG_GET_DIG_Z cases:
// save parameters for temperature compensation.
MAG_READ(BMM150_REG_DIG_X1, 8);
mTask.mag_state = MAG_GET_DIG_Y;
break;
case MAG_GET_DIG_Y:
bmm150SaveDigData(&magTask, &mTask.dataBuffer[1], 0);
MAG_READ(BMM150_REG_DIG_X1 + 8, 8);
mTask.mag_state = MAG_GET_DIG_Z;
break;
case MAG_GET_DIG_Z:
bmm150SaveDigData(&magTask, &mTask.dataBuffer[1], 8);
MAG_READ(BMM150_REG_DIG_X1 + 16, 8);
mTask.mag_state = MAG_SET_SAVE_DIG;
break;
case MAG_SET_SAVE_DIG:
bmm150SaveDigData(&magTask, &mTask.dataBuffer[1], 16);
// fall through, no break;
mTask.mag_state = MAG_SET_FORCE;
#endif
case MAG_SET_FORCE:
// set MAG mode to "forced". ready to pull data
#ifdef USE_AK09915
MAG_WRITE(AKM_AK09915_REG_CNTL2, 0x01);
#elif USE_BMM150
MAG_WRITE(BMM150_REG_CTRL_2, 0x02);
#endif
mTask.mag_state = MAG_SET_ADDR;
break;
case MAG_SET_ADDR:
// config MAG read data address to the first data register
#ifdef MAG_SLAVE_PRESENT
SPI_WRITE(BMI160_REG_MAG_IF_2, MAG_REG_DATA);
#endif
mTask.mag_state = MAG_SET_DATA;
break;
case MAG_SET_DATA:
// clear mag_manual_en.
SPI_WRITE(BMI160_REG_MAG_IF_1, 0x03, 1000);
// set the MAG power to SUSPEND mode
SPI_WRITE(BMI160_REG_CMD, 0x18, 10000);
mTask.mag_state = MAG_SET_DONE;
mTask.init_state = INIT_ON_CHANGE_SENSORS;
break;
default:
break;
}
SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 1000);
}
static inline bool anyFifoEnabled(void)
{
bool anyFifoEnabled = mTask.fifo_enabled[ACC] || mTask.fifo_enabled[GYR];
#ifdef MAG_SLAVE_PRESENT
anyFifoEnabled = anyFifoEnabled || mTask.fifo_enabled[MAG];
#endif
return anyFifoEnabled;
}
static void configFifo(void)
{
TDECL();
int i;
uint8_t val = 0x12;
bool any_fifo_enabled_prev = anyFifoEnabled();
// if ACC is configed, enable ACC bit in fifo_config reg.
if (mTask.sensors[ACC].configed && mTask.sensors[ACC].latency != SENSOR_LATENCY_NODATA) {
val |= 0x40;
mTask.fifo_enabled[ACC] = true;
} else {
mTask.fifo_enabled[ACC] = false;
}
// if GYR is configed, enable GYR bit in fifo_config reg.
if (mTask.sensors[GYR].configed && mTask.sensors[GYR].latency != SENSOR_LATENCY_NODATA) {
val |= 0x80;
mTask.fifo_enabled[GYR] = true;
} else {
mTask.fifo_enabled[GYR] = false;
}
#ifdef MAG_SLAVE_PRESENT
// if MAG is configed, enable MAG bit in fifo_config reg.
if (mTask.sensors[MAG].configed && mTask.sensors[MAG].latency != SENSOR_LATENCY_NODATA) {
val |= 0x20;
mTask.fifo_enabled[MAG] = true;
} else {
mTask.fifo_enabled[MAG] = false;
}
#endif
// if this is the first data sensor fifo to enable, start to
// sync the sensor time and rtc time
if (!any_fifo_enabled_prev && anyFifoEnabled()) {
invalidate_sensortime_to_rtc_time();
// start a new poll generation and attach the generation number to event
osEnqueuePrivateEvt(EVT_TIME_SYNC, (void *)mTask.poll_generation, NULL, mTask.tid);
}
// cancel current poll generation
if (any_fifo_enabled_prev && !anyFifoEnabled()) {
++mTask.poll_generation;
}
// if this is not the first fifo enabled or last fifo disabled, flush all fifo data;
if (any_fifo_enabled_prev && anyFifoEnabled()) {
mTask.pending_dispatch = true;
mTask.xferCnt = FIFO_READ_SIZE;
SPI_READ(BMI160_REG_FIFO_DATA, mTask.xferCnt, &mTask.dataBuffer);
}
// calculate the new watermark level
if (anyFifoEnabled()) {
mTask.watermark = calcWatermark2_(_task);
DEBUG_PRINT("wm=%d", mTask.watermark);
SPI_WRITE(BMI160_REG_FIFO_CONFIG_0, mTask.watermark);
}
// config the fifo register
SPI_WRITE(BMI160_REG_FIFO_CONFIG_1, val);
// if no more fifo enabled, we need to cleanup the fifo and invalidate time
if (!anyFifoEnabled()) {
SPI_WRITE(BMI160_REG_CMD, 0xb0);
mTask.frame_sensortime_valid = false;
for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; i++) {
mTask.pending_delta[i] = false;
mTask.prev_frame_time[i] = ULONG_LONG_MAX;
}
}
}
static bool accPower(bool on, void *cookie)
{
TDECL();
INFO_PRINT("accPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE()));
if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
if (on) {
// set ACC power mode to NORMAL
SPI_WRITE(BMI160_REG_CMD, 0x11, 50000);
} else {
// set ACC power mode to SUSPEND
mTask.sensors[ACC].configed = false;
configFifo();
SPI_WRITE(BMI160_REG_CMD, 0x10, 5000);
}
mTask.sensors[ACC].powered = on;
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
} else {
mTask.pending_config[ACC] = true;
mTask.sensors[ACC].pConfig.enable = on;
}
return true;
}
static bool gyrPower(bool on, void *cookie)
{
TDECL();
INFO_PRINT("gyrPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE()));
if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
if (on) {
// set GYR power mode to NORMAL
SPI_WRITE(BMI160_REG_CMD, 0x15, 50000);
} else {
// set GYR power mode to SUSPEND
mTask.sensors[GYR].configed = false;
configFifo();
SPI_WRITE(BMI160_REG_CMD, 0x14, 5000);
}
if (anyFifoEnabled() && on != mTask.sensors[GYR].powered) {
#if TIMESTAMP_DBG
DEBUG_PRINT("minimize_sensortime_history()\n");
#endif
minimize_sensortime_history();
}
mTask.sensors[GYR].powered = on;
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
} else {
mTask.pending_config[GYR] = true;
mTask.sensors[GYR].pConfig.enable = on;
}
return true;
}
#ifdef MAG_SLAVE_PRESENT
static bool magPower(bool on, void *cookie)
{
TDECL();
INFO_PRINT("magPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE()));
if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
if (on) {
// set MAG power mode to NORMAL
SPI_WRITE(BMI160_REG_CMD, 0x19, 10000);
} else {
// set MAG power mode to SUSPEND
mTask.sensors[MAG].configed = false;
configFifo();
SPI_WRITE(BMI160_REG_CMD, 0x18, 5000);
}
mTask.sensors[MAG].powered = on;
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[MAG], __FUNCTION__);
} else {
mTask.pending_config[MAG] = true;
mTask.sensors[MAG].pConfig.enable = on;
}
return true;
}
#endif
static bool stepPower(bool on, void *cookie)
{
TDECL();
if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
// if step counter is powered, no need to change actual config of step
// detector.
// But we choose to perform one SPI_WRITE anyway to go down the code path
// to state SENSOR_POWERING_UP/DOWN to update sensor manager.
if (on) {
mTask.interrupt_enable_2 |= 0x08;
} else {
if (!mTask.sensors[STEPCNT].powered)
mTask.interrupt_enable_2 &= ~0x08;
mTask.sensors[STEP].configed = false;
}
mTask.sensors[STEP].powered = on;
SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2, 450);
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEP], __FUNCTION__);
} else {
mTask.pending_config[STEP] = true;
mTask.sensors[STEP].pConfig.enable = on;
}
return true;
}
static bool flatPower(bool on, void *cookie)
{
TDECL();
if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
if (on) {
mTask.interrupt_enable_0 |= 0x80;
} else {
mTask.interrupt_enable_0 &= ~0x80;
mTask.sensors[FLAT].configed = false;
}
mTask.sensors[FLAT].powered = on;
SPI_WRITE(BMI160_REG_INT_EN_0, mTask.interrupt_enable_0, 450);
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[FLAT], __FUNCTION__);
} else {
mTask.pending_config[FLAT] = true;
mTask.sensors[FLAT].pConfig.enable = on;
}
return true;
}
static bool doubleTapPower(bool on, void *cookie)
{
TDECL();
if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
if (on) {
mTask.interrupt_enable_0 |= 0x10;
} else {
mTask.interrupt_enable_0 &= ~0x10;
mTask.sensors[DTAP].configed = false;
}
mTask.sensors[DTAP].powered = on;
SPI_WRITE(BMI160_REG_INT_EN_0, mTask.interrupt_enable_0, 450);
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[DTAP], __FUNCTION__);
} else {
mTask.pending_config[DTAP] = true;
mTask.sensors[DTAP].pConfig.enable = on;
}
return true;
}
static bool anyMotionPower(bool on, void *cookie)
{
TDECL();
DEBUG_PRINT("anyMotionPower: on=%d, oneshot_cnt %d, state=%" PRI_STATE "\n",
on, mTask.active_oneshot_sensor_cnt, getStateName(GET_STATE()));
if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
if (on) {
mTask.interrupt_enable_0 |= 0x07;
} else {
mTask.interrupt_enable_0 &= ~0x07;
mTask.sensors[ANYMO].configed = false;
}
mTask.sensors[ANYMO].powered = on;
SPI_WRITE(BMI160_REG_INT_EN_0, mTask.interrupt_enable_0, 450);
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ANYMO], __FUNCTION__);
} else {
mTask.pending_config[ANYMO] = true;
mTask.sensors[ANYMO].pConfig.enable = on;
}
return true;
}
static bool noMotionPower(bool on, void *cookie)
{
TDECL();
DEBUG_PRINT("noMotionPower: on=%d, oneshot_cnt %d, state=%" PRI_STATE "\n",
on, mTask.active_oneshot_sensor_cnt, getStateName(GET_STATE()));
if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
if (on) {
mTask.interrupt_enable_2 |= 0x07;
} else {
mTask.interrupt_enable_2 &= ~0x07;
mTask.sensors[NOMO].configed = false;
}
mTask.sensors[NOMO].powered = on;
SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2, 450);
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[NOMO], __FUNCTION__);
} else {
mTask.pending_config[NOMO] = true;
mTask.sensors[NOMO].pConfig.enable = on;
}
return true;
}
static bool stepCntPower(bool on, void *cookie)
{
TDECL();
if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
if (on) {
if (!mTask.sensors[STEP].powered) {
mTask.interrupt_enable_2 |= 0x08;
SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2, 450);
}
// set step_cnt_en bit
SPI_WRITE(BMI160_REG_STEP_CONF_1, 0x08 | 0x03, 1000);
} else {
if (mTask.stepCntSamplingTimerHandle) {
timTimerCancel(mTask.stepCntSamplingTimerHandle);
mTask.stepCntSamplingTimerHandle = 0;
}
if (!mTask.sensors[STEP].powered) {
mTask.interrupt_enable_2 &= ~0x08;
SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2);
}
// unset step_cnt_en bit
SPI_WRITE(BMI160_REG_STEP_CONF_1, 0x03);
mTask.last_step_cnt = 0;
mTask.sensors[STEPCNT].configed = false;
}
mTask.sensors[STEPCNT].powered = on;
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEPCNT], __FUNCTION__);
} else {
mTask.pending_config[STEPCNT] = true;
mTask.sensors[STEPCNT].pConfig.enable = on;
}
return true;
}
static void updateTimeDelta(uint8_t idx, uint8_t odr)
{
if (mTask.fifo_enabled[idx]) {
// wait till control frame to update, if not disabled
mTask.next_delta[idx] = 1ull << (16 - odr);
mTask.pending_delta[idx] = true;
} else {
mTask.time_delta[idx] = 1ull << (16 - odr);
}
}
// compute the register value from sensor rate.
static uint8_t computeOdr(uint32_t rate)
{
uint8_t odr = 0x00;
switch (rate) {
// fall through intended to get the correct register value
case SENSOR_HZ(3200): odr ++;
case SENSOR_HZ(1600): odr ++;
case SENSOR_HZ(800): odr ++;
case SENSOR_HZ(400): odr ++;
case SENSOR_HZ(200): odr ++;
case SENSOR_HZ(100): odr ++;
case SENSOR_HZ(50): odr ++;
case SENSOR_HZ(25): odr ++;
case SENSOR_HZ(25.0f/2.0f): odr ++;
case SENSOR_HZ(25.0f/4.0f): odr ++;
case SENSOR_HZ(25.0f/8.0f): odr ++;
case SENSOR_HZ(25.0f/16.0f): odr ++;
case SENSOR_HZ(25.0f/32.0f): odr ++;
default:
return odr;
}
}
static void configMotion(uint8_t odr) {
// motion threshold is element * 15.63mg (for 8g range)
static const uint8_t motion_thresholds[ACC_MAX_RATE+1] =
{5, 5, 5, 5, 5, 5, 5, 5, 4, 3, 2, 2, 2};
// set any_motion duration to 1 point
// set no_motion duration to (3+1)*1.28sec=5.12sec
SPI_WRITE(BMI160_REG_INT_MOTION_0, 0x03 << 2, 450);
// set any_motion threshold
SPI_WRITE(BMI160_REG_INT_MOTION_1, motion_thresholds[odr], 450);
// set no_motion threshold
SPI_WRITE(BMI160_REG_INT_MOTION_2, motion_thresholds[odr], 450);
}
static bool accSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
TDECL();
int odr, osr = 0;
INFO_PRINT("accSetRate: rate=%ld, latency=%lld, state=%" PRI_STATE "\n", rate, latency,
getStateName(GET_STATE()));
if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
odr = computeOdr(rate);
if (!odr) {
ERROR_PRINT("invalid acc rate\n");
return false;
}
updateTimeDelta(ACC, odr);
// minimum supported rate for ACCEL is 12.5Hz.
// Anything lower than that shall be acheived by downsampling.
if (odr < ACC_MIN_RATE) {
osr = ACC_MIN_RATE - odr;
odr = ACC_MIN_RATE;
}
// for high odrs, oversample to reduce hw latency and downsample
// to get desired odr
if (odr > OSR_THRESHOLD) {
osr = (ACC_MAX_OSR + odr) > ACC_MAX_RATE ? (ACC_MAX_RATE - odr) : ACC_MAX_OSR;
odr += osr;
}
mTask.sensors[ACC].rate = rate;
mTask.sensors[ACC].latency = latency;
mTask.sensors[ACC].configed = true;
mTask.acc_downsample = osr;
// configure ANY_MOTION and NO_MOTION based on odr
configMotion(odr);
// set ACC bandwidth parameter to 2 (bits[4:6])
// set the rate (bits[0:3])
SPI_WRITE(BMI160_REG_ACC_CONF, 0x20 | odr);
// configure down sampling ratio, 0x88 is to specify we are using
// filtered samples
SPI_WRITE(BMI160_REG_FIFO_DOWNS, (mTask.acc_downsample << 4) | mTask.gyr_downsample | 0x88);
// flush the data and configure the fifo
configFifo();
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
} else {
mTask.pending_config[ACC] = true;
mTask.sensors[ACC].pConfig.enable = 1;
mTask.sensors[ACC].pConfig.rate = rate;
mTask.sensors[ACC].pConfig.latency = latency;
}
return true;
}
static bool gyrSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
TDECL();
int odr, osr = 0;
INFO_PRINT("gyrSetRate: rate=%ld, latency=%lld, state=%" PRI_STATE "\n", rate, latency,
getStateName(GET_STATE()));
if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
odr = computeOdr(rate);
if (!odr) {
ERROR_PRINT("invalid gyr rate\n");
return false;
}
updateTimeDelta(GYR, odr);
// minimum supported rate for GYRO is 25.0Hz.
// Anything lower than that shall be acheived by downsampling.
if (odr < GYR_MIN_RATE) {
osr = GYR_MIN_RATE - odr;
odr = GYR_MIN_RATE;
}
// for high odrs, oversample to reduce hw latency and downsample
// to get desired odr
if (odr > OSR_THRESHOLD) {
osr = (GYR_MAX_OSR + odr) > GYR_MAX_RATE ? (GYR_MAX_RATE - odr) : GYR_MAX_OSR;
odr += osr;
}
mTask.sensors[GYR].rate = rate;
mTask.sensors[GYR].latency = latency;
mTask.sensors[GYR].configed = true;
mTask.gyr_downsample = osr;
// set GYR bandwidth parameter to 2 (bits[4:6])
// set the rate (bits[0:3])
SPI_WRITE(BMI160_REG_GYR_CONF, 0x20 | odr);
// configure down sampling ratio, 0x88 is to specify we are using
// filtered samples
SPI_WRITE(BMI160_REG_FIFO_DOWNS, (mTask.acc_downsample << 4) | mTask.gyr_downsample | 0x88);
// flush the data and configure the fifo
configFifo();
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
} else {
mTask.pending_config[GYR] = true;
mTask.sensors[GYR].pConfig.enable = 1;
mTask.sensors[GYR].pConfig.rate = rate;
mTask.sensors[GYR].pConfig.latency = latency;
}
return true;
}
#ifdef MAG_SLAVE_PRESENT
static bool magSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
TDECL();
int odr;
if (rate == SENSOR_RATE_ONCHANGE)
rate = SENSOR_HZ(100);
INFO_PRINT("magSetRate: rate=%ld, latency=%lld, state=%" PRI_STATE "\n", rate, latency,
getStateName(GET_STATE()));
if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
mTask.sensors[MAG].rate = rate;
mTask.sensors[MAG].latency = latency;
mTask.sensors[MAG].configed = true;
odr = computeOdr(rate);
if (!odr) {
ERROR_PRINT("invalid mag rate\n");
return false;
}
updateTimeDelta(MAG, odr);
odr = odr > MAG_MAX_RATE ? MAG_MAX_RATE : odr;
// set the rate for MAG
SPI_WRITE(BMI160_REG_MAG_CONF, odr);
// flush the data and configure the fifo
configFifo();
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[MAG], __FUNCTION__);
} else {
mTask.pending_config[MAG] = true;
mTask.sensors[MAG].pConfig.enable = 1;
mTask.sensors[MAG].pConfig.rate = rate;
mTask.sensors[MAG].pConfig.latency = latency;
}
return true;
}
#endif
static bool stepSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
mTask.sensors[STEP].rate = rate;
mTask.sensors[STEP].latency = latency;
mTask.sensors[STEP].configed = true;
sensorSignalInternalEvt(mTask.sensors[STEP].handle,
SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
return true;
}
static bool flatSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
mTask.sensors[FLAT].rate = rate;
mTask.sensors[FLAT].latency = latency;
mTask.sensors[FLAT].configed = true;
sensorSignalInternalEvt(mTask.sensors[FLAT].handle,
SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
return true;
}
static bool doubleTapSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
mTask.sensors[DTAP].rate = rate;
mTask.sensors[DTAP].latency = latency;
mTask.sensors[DTAP].configed = true;
sensorSignalInternalEvt(mTask.sensors[DTAP].handle,
SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
return true;
}
static bool anyMotionSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
mTask.sensors[ANYMO].rate = rate;
mTask.sensors[ANYMO].latency = latency;
mTask.sensors[ANYMO].configed = true;
sensorSignalInternalEvt(mTask.sensors[ANYMO].handle,
SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
return true;
}
static bool noMotionSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
mTask.sensors[NOMO].rate = rate;
mTask.sensors[NOMO].latency = latency;
mTask.sensors[NOMO].configed = true;
sensorSignalInternalEvt(mTask.sensors[NOMO].handle,
SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
return true;
}
static bool stepCntSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
mTask.sensors[STEPCNT].rate = rate;
mTask.sensors[STEPCNT].latency = latency;
mTask.sensors[STEPCNT].configed = true;
if (rate == SENSOR_RATE_ONCHANGE && mTask.stepCntSamplingTimerHandle) {
timTimerCancel(mTask.stepCntSamplingTimerHandle);
mTask.stepCntSamplingTimerHandle = 0;
} else if (rate != SENSOR_RATE_ONCHANGE) {
if (mTask.stepCntSamplingTimerHandle) {
timTimerCancel(mTask.stepCntSamplingTimerHandle);
}
mTask.stepCntSamplingTimerHandle = timTimerSet(sensorTimerLookupCommon(StepCntRates, stepCntRateTimerVals, rate),
0, 50, stepCntSamplingCallback, NULL, false);
}
sensorSignalInternalEvt(mTask.sensors[STEPCNT].handle,
SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
return true;
}
static void sendFlushEvt(void)
{
while (mTask.sensors[ACC].flush > 0) {
osEnqueueEvt(EVT_SENSOR_ACC_DATA_RDY, SENSOR_DATA_EVENT_FLUSH, NULL);
mTask.sensors[ACC].flush--;
}
while (mTask.sensors[GYR].flush > 0) {
osEnqueueEvt(EVT_SENSOR_GYR_DATA_RDY, SENSOR_DATA_EVENT_FLUSH, NULL);
mTask.sensors[GYR].flush--;
}
#ifdef MAG_SLAVE_PRESENT
while (mTask.sensors[MAG].flush > 0) {
osEnqueueEvt(EVT_SENSOR_MAG_DATA_RDY, SENSOR_DATA_EVENT_FLUSH, NULL);
mTask.sensors[MAG].flush--;
}
#endif
}
static bool accFlush(void *cookie)
{
TDECL();
mTask.sensors[ACC].flush++;
initiateFifoRead(false /*isInterruptContext*/);
return true;
}
static bool gyrFlush(void *cookie)
{
TDECL();
mTask.sensors[GYR].flush++;
initiateFifoRead(false /*isInterruptContext*/);
return true;
}
#ifdef MAG_SLAVE_PRESENT
static bool magFlush(void *cookie)
{
TDECL();
mTask.sensors[MAG].flush++;
initiateFifoRead(false /*isInterruptContext*/);
return true;
}
#endif
static bool stepFlush(void *cookie)
{
return osEnqueueEvt(EVT_SENSOR_STEP, SENSOR_DATA_EVENT_FLUSH, NULL);
}
static bool flatFlush(void *cookie)
{
return osEnqueueEvt(EVT_SENSOR_FLAT, SENSOR_DATA_EVENT_FLUSH, NULL);
}
static bool doubleTapFlush(void *cookie)
{
return osEnqueueEvt(EVT_SENSOR_DOUBLE_TAP, SENSOR_DATA_EVENT_FLUSH, NULL);
}
static bool anyMotionFlush(void *cookie)
{
return osEnqueueEvt(EVT_SENSOR_ANY_MOTION, SENSOR_DATA_EVENT_FLUSH, NULL);
}
static bool noMotionFlush(void *cookie)
{
return osEnqueueEvt(EVT_SENSOR_NO_MOTION, SENSOR_DATA_EVENT_FLUSH, NULL);
}
static bool stepCntFlushGetData()
{
TDECL();
if (trySwitchState(SENSOR_STEP_CNT)) {
SPI_READ(BMI160_REG_STEP_CNT_0, 2, &mTask.dataBuffer);
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEPCNT], __FUNCTION__);
return true;
}
return false;
}
static bool stepCntFlush(void *cookie)
{
mTask.sensors[STEPCNT].flush++;
stepCntFlushGetData();
return true;
}
static void sendStepCnt()
{
union EmbeddedDataPoint step_cnt;
uint32_t cur_step_cnt;
cur_step_cnt = (int)(mTask.dataBuffer[1] | (mTask.dataBuffer[2] << 8));
if (cur_step_cnt != mTask.last_step_cnt) {
// Check for possible overflow
if (cur_step_cnt < mTask.last_step_cnt) {
mTask.total_step_cnt += cur_step_cnt + (0xFFFF - mTask.last_step_cnt);
} else {
mTask.total_step_cnt += (cur_step_cnt - mTask.last_step_cnt);
}
mTask.last_step_cnt = cur_step_cnt;
// Send the event if the current rate is ONCHANGE or we need to flush;
// otherwise, wait until step count sampling timer expires
if (mTask.sensors[STEPCNT].rate == SENSOR_RATE_ONCHANGE || mTask.sensors[STEPCNT].flush) {
step_cnt.idata = mTask.total_step_cnt;
osEnqueueEvt(EVT_SENSOR_STEP_COUNTER, step_cnt.vptr, NULL);
} else {
mTask.step_cnt_changed = true;
}
}
while (mTask.sensors[STEPCNT].flush) {
osEnqueueEvt(EVT_SENSOR_STEP_COUNTER, SENSOR_DATA_EVENT_FLUSH, NULL);
mTask.sensors[STEPCNT].flush--;
}
}
static bool stepCntSendLastData(void *cookie, uint32_t tid)
{
// If this comes in and we don't have data yet, there's no harm in reporting step_cnt = 0
return osEnqueuePrivateEvt(EVT_SENSOR_STEP_COUNTER, (void *) mTask.total_step_cnt, NULL, tid);
}
static uint64_t parseSensortime(uint32_t sensor_time24)
{
uint32_t prev_time24;
uint32_t kHalf = 1ul << 23;
uint64_t full;
prev_time24 = (uint32_t)mTask.last_sensortime & 0xffffff;
if (mTask.last_sensortime == 0) {
mTask.last_sensortime = (uint64_t)sensor_time24;
return (uint64_t)(sensor_time24);
}
if (sensor_time24 == prev_time24) {
return (uint64_t)(mTask.last_sensortime);
}
full = (mTask.last_sensortime & ~0xffffffull) | sensor_time24;
if (((prev_time24 < sensor_time24) && (sensor_time24 - prev_time24) < kHalf)
|| ((prev_time24 > sensor_time24) && (prev_time24 - sensor_time24) > kHalf)) {
if (full < mTask.last_sensortime) {
full += 0x1000000ull;
}
mTask.last_sensortime = full;
return mTask.last_sensortime;
}
if (full < mTask.last_sensortime) {
return full;
}
return (full - 0x1000000ull);
}
static bool flushData(struct BMI160Sensor *sensor, uint32_t eventId)
{
bool success = false;
if (sensor->data_evt) {
success = osEnqueueEvtOrFree(eventId, sensor->data_evt, dataEvtFree);
sensor->data_evt = NULL;
}
return success;
}
static void flushAllData(void)
{
int i;
for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; i++) {
flushData(&mTask.sensors[i],
EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[i].sensorType));
}
}
static bool allocateDataEvt(struct BMI160Sensor *mSensor, uint64_t rtc_time)
{
TDECL();
mSensor->data_evt = slabAllocatorAlloc(T(mDataSlab));
if (mSensor->data_evt == NULL) {
// slab allocation failed
ERROR_PRINT("slabAllocatorAlloc() failed\n");
return false;
}
// delta time for the first sample is sample count
memset(&mSensor->data_evt->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample));
mSensor->data_evt->referenceTime = rtc_time;
mSensor->prev_rtc_time = rtc_time;
return true;
}
static void parseRawData(struct BMI160Sensor *mSensor, uint8_t *buf, float kScale, uint64_t sensorTime)
{
float x, y, z;
int16_t raw_x, raw_y, raw_z;
struct TripleAxisDataPoint *sample;
uint32_t delta_time;
uint64_t rtc_time;
#ifdef MAG_SLAVE_PRESENT
bool newMagBias = false;
#endif
if (!sensortime_to_rtc_time(sensorTime, &rtc_time)) {
return;
}
if (rtc_time < mSensor->prev_rtc_time + kMinRTCTimeIncrementNs) {
#if TIMESTAMP_DBG
DEBUG_PRINT("%s prev rtc 0x%08x %08x, curr 0x%08x %08x, delta %d usec\n",
mSensorInfo[mSensor->idx].sensorName,
(unsigned int)((mSensor->prev_rtc_time >> 32) & 0xffffffff),
(unsigned int)(mSensor->prev_rtc_time & 0xffffffff),
(unsigned int)((rtc_time >> 32) & 0xffffffff),
(unsigned int)(rtc_time & 0xffffffff),
(int)(rtc_time - mSensor->prev_rtc_time) / 1000);
#endif
rtc_time = mSensor->prev_rtc_time + kMinRTCTimeIncrementNs;
}
#ifdef MAG_SLAVE_PRESENT
if (mSensor->idx == MAG) {
parseMagData(&magTask, &buf[0], &x, &y, &z);
BMM150_TO_ANDROID_COORDINATE(x, y, z);
float xi, yi, zi;
magCalRemoveSoftiron(&mTask.moc, x, y, z, &xi, &yi, &zi);
newMagBias |= magCalUpdate(&mTask.moc, sensorTime * kSensorTimerIntervalUs, xi, yi, zi);
magCalRemoveBias(&mTask.moc, xi, yi, zi, &x, &y, &z);
#ifdef GYRO_CAL_ENABLED
// Gyro Cal -- Add magnetometer sample.
gyroCalUpdateMag(&mTask.gyro_cal,
rtc_time, //nsec
x, y, z);
#endif
} else
#endif
{
raw_x = (buf[0] | buf[1] << 8);
raw_y = (buf[2] | buf[3] << 8);
raw_z = (buf[4] | buf[5] << 8);
x = (float)raw_x * kScale;
y = (float)raw_y * kScale;
z = (float)raw_z * kScale;
BMI160_TO_ANDROID_COORDINATE(x, y, z);
if (mSensor->idx == ACC) {
#ifdef ACCEL_CAL_ENABLED
accelCalRun(&mTask.acc, rtc_time,
x, y, z, mTask.tempCelsius);
accelCalBiasRemove(&mTask.acc, &x, &y, &z);
#endif
#ifdef GYRO_CAL_ENABLED
// Gyro Cal -- Add accelerometer sample.
gyroCalUpdateAccel(&mTask.gyro_cal,
rtc_time, //nsec
x, y, z);
#endif
} else {
if (mSensor->idx == GYR) {
#ifdef GYRO_CAL_ENABLED
// Gyro Cal -- Add gyroscope and temperature sample.
gyroCalUpdateGyro(&mTask.gyro_cal,
rtc_time, //nsec
x, y, z,
mTask.tempCelsius);
// Gyro Cal -- Apply calibration correction.
gyroCalRemoveBias(&mTask.gyro_cal,
x, y, z, //input values
&x, &y, &z //calibrated output
);
#endif
}
}
}
if (mSensor->data_evt == NULL) {
if (!allocateDataEvt(mSensor, rtc_time))
return;
}
if (mSensor->data_evt->samples[0].firstSample.numSamples >= MAX_NUM_COMMS_EVENT_SAMPLES) {
ERROR_PRINT("BAD INDEX\n");
return;
}
#ifdef MAG_SLAVE_PRESENT
if (mSensor->idx == MAG && (newMagBias || !mTask.magBiasPosted)) {
#ifdef GYRO_CAL_ENABLED
#ifdef GYRO_CAL_DBG_ENABLED
// Gyro Cal -- Read out Debug data.
if (mTask.gyro_debug_state < 0) {
mTask.gyro_debug_state = 0; //kick off the debug print out.
}
#endif
#endif
if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) {
// flush existing samples so the bias appears after them
flushData(mSensor,
EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[MAG].sensorType));
if (!allocateDataEvt(mSensor, rtc_time))
return;
}
if (newMagBias)
mTask.magBiasCurrent = true;
mSensor->data_evt->samples[0].firstSample.biasCurrent = mTask.magBiasCurrent;
mSensor->data_evt->samples[0].firstSample.biasPresent = 1;
mSensor->data_evt->samples[0].firstSample.biasSample =
mSensor->data_evt->samples[0].firstSample.numSamples;
sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
magCalGetBias(&mTask.moc, &sample->x, &sample->y, &sample->z);
// bias is non-discardable, if we fail to enqueue, don't clear new_mag_bias
if (flushData(mSensor, sensorGetMyEventType(mSensorInfo[MAG].biasType)))
mTask.magBiasPosted = true;
if (!allocateDataEvt(mSensor, rtc_time))
return;
}
#endif
#ifdef GYRO_CAL_ENABLED
// Gyro Cal -- Notify HAL about new gyro bias calibration
if (mSensor->idx == GYR && gyroCalNewBiasAvailable(&mTask.gyro_cal)) {
if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) {
// flush existing samples so the bias appears after them
flushData(mSensor,
EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[GYR].sensorType));
if (!allocateDataEvt(mSensor, rtc_time))
return;
}
mSensor->data_evt->samples[0].firstSample.biasCurrent = true;
mSensor->data_evt->samples[0].firstSample.biasPresent = 1;
mSensor->data_evt->samples[0].firstSample.biasSample =
mSensor->data_evt->samples[0].firstSample.numSamples;
sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
gyroCalGetBias(&mTask.gyro_cal, &sample->x, &sample->y, &sample->z);
flushData(mSensor, sensorGetMyEventType(mSensorInfo[GYR].biasType));
if (!allocateDataEvt(mSensor, rtc_time))
return;
}
#endif
sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
// the first deltatime is for sample size
if (mSensor->data_evt->samples[0].firstSample.numSamples > 1) {
delta_time = rtc_time - mSensor->prev_rtc_time;
delta_time = delta_time < 0 ? 0 : delta_time;
sample->deltaTime = delta_time;
mSensor->prev_rtc_time = rtc_time;
}
sample->x = x;
sample->y = y;
sample->z = z;
//DEBUG_PRINT("bmi160: x: %d, y: %d, z: %d\n", (int)(1000*x), (int)(1000*y), (int)(1000*z));
//TODO: This was added to prevent to much data of the same type accumulate in internal buffer.
// It might no longer be necessary and can be removed.
if (mSensor->data_evt->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) {
flushAllData();
}
}
static void dispatchData(void)
{
size_t i = 1, j;
size_t size = mTask.xferCnt;
int fh_mode, fh_param;
uint8_t *buf = mTask.dataBuffer;
uint64_t min_delta = ULONG_LONG_MAX;
uint32_t sensor_time24;
uint64_t full_sensor_time;
uint64_t frame_sensor_time = mTask.frame_sensortime;
bool observed[NUM_CONT_SENSOR];
uint64_t tmp_frame_time, tmp_time[NUM_CONT_SENSOR];
bool frame_sensor_time_valid = mTask.frame_sensortime_valid;
bool saved_pending_delta[NUM_CONT_SENSOR];
uint64_t saved_time_delta[NUM_CONT_SENSOR];
#if TIMESTAMP_DBG
int frame_num = -1;
#endif
for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++)
observed[j] = false;
if (!mTask.frame_sensortime_valid) {
// This is the first FIFO delivery after any sensor is enabled in
// bmi160. Sensor time reference is not establised until end of this
// FIFO frame. Assume time start from zero and do a dry run to estimate
// the time and then go through this FIFO again.
frame_sensor_time = 0ull;
// Save these states for future recovery by the end of dry run.
for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) {
saved_pending_delta[j] = mTask.pending_delta[j];
saved_time_delta[j] = mTask.time_delta[j];
}
}
while (size > 0) {
if (buf[i] == BMI160_FRAME_HEADER_INVALID) {
// reaching invalid header means no more data
break;
} else if (buf[i] == BMI160_FRAME_HEADER_SKIP) {
// manually injected skip header
DEBUG_PRINT_IF(DBG_CHUNKED, "skip nop header");
i++;
size--;
continue;
}
fh_mode = buf[i] >> 6;
fh_param = (buf[i] >> 2) & 0xf;
i++;
size--;
#if TIMESTAMP_DBG
++frame_num;
#endif
if (fh_mode == 1) {
// control frame.
if (fh_param == 0) {
// skip frame, we skip it
if (size >= 1) {
i++;
size--;
} else {
size = 0;
}
} else if (fh_param == 1) {
// sensortime frame
if (size >= 3) {
// The active sensor with the highest odr/lowest delta is the one that
// determines the sensor time increments.
for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) {
if (mTask.sensors[j].configed &&
mTask.sensors[j].latency != SENSOR_LATENCY_NODATA) {
min_delta = min_delta < mTask.time_delta[j] ? min_delta :
mTask.time_delta[j];
}
}
sensor_time24 = buf[i + 2] << 16 | buf[i + 1] << 8 | buf[i];
// clear lower bits that measure time from taking the sample to reading the
// FIFO, something we're not interested in.
sensor_time24 &= ~(min_delta - 1);
full_sensor_time = parseSensortime(sensor_time24);
#if TIMESTAMP_DBG
if (frame_sensor_time == full_sensor_time) {
//DEBUG_PRINT("frame %d FrameTime 0x%08x\n",
// frame_num - 1,
// (unsigned int)frame_sensor_time);
} else if (frame_sensor_time_valid) {
DEBUG_PRINT("frame %d FrameTime 0x%08x != SensorTime 0x%08x, jumped %d msec\n",
frame_num - 1,
(unsigned int)frame_sensor_time,
(unsigned int)full_sensor_time,
(int)(5 * ((int64_t)(full_sensor_time - frame_sensor_time) >> 7)));
}
#endif
if (frame_sensor_time_valid) {
mTask.frame_sensortime = full_sensor_time;
} else {
// Dry run if frame_sensortime_valid == false,
// no sample is added this round.
// So let's time travel back to beginning of frame.
mTask.frame_sensortime_valid = true;
mTask.frame_sensortime = full_sensor_time - frame_sensor_time;
// recover states
for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) {
// reset all prev_frame_time to invalid values
// they should be so anyway at the first FIFO
mTask.prev_frame_time[j] = ULONG_LONG_MAX;
// recover saved time_delta and pending_delta values
mTask.pending_delta[j] = saved_pending_delta[j];
mTask.time_delta[j] = saved_time_delta[j];
}
DEBUG_PRINT_IF(TIMESTAMP_DBG,
"sensortime invalid: full, frame, task = %llu, %llu, %llu\n",
full_sensor_time,
frame_sensor_time,
mTask.frame_sensortime);
// Parse again with known valid timing.
// This time the sensor events will be committed into event buffer.
return dispatchData();
}
// Invalidate sensor timestamp that didn't get corrected by full_sensor_time,
// so it can't be used as a reference at next FIFO read.
// Use (ULONG_LONG_MAX - 1) to indicate this.
for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) {
mTask.prev_frame_time[j] = observed[j] ? full_sensor_time : (ULONG_LONG_MAX - 1);
// sensor can be disabled in the middle of the FIFO, but wait till the FIFO
// end to invalidate prev_frame_time since it's still needed for parsing.
// Also invalidate pending delta just to be safe.
if (!mTask.sensors[j].configed ||
mTask.sensors[j].latency == SENSOR_LATENCY_NODATA) {
mTask.prev_frame_time[j] = ULONG_LONG_MAX;
mTask.pending_delta[j] = false;
}
}
i += 3;
size -= 3;
} else {
size = 0;
}
} else if (fh_param == 2) {
// fifo_input config frame
#if TIMESTAMP_DBG
DEBUG_PRINT("frame %d config change 0x%02x\n", frame_num, buf[i]);
#endif
if (size >= 1) {
for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) {
if (buf[i] & (0x01 << (j << 1)) && mTask.pending_delta[j]) {
mTask.pending_delta[j] = false;
mTask.time_delta[j] = mTask.next_delta[j];
#if TIMESTAMP_DBG
DEBUG_PRINT("%s new delta %u\n", mSensorInfo[j].sensorName,
(unsigned int)mTask.time_delta[j]);
#endif
}
}
i++;
size--;
} else {
size = 0;
}
} else {
size = 0; // drop this batch
ERROR_PRINT("Invalid fh_param in conttrol frame\n");
}
} else if (fh_mode == 2) {
// Calcutate candidate frame time (tmp_frame_time):
// 1) When sensor is first enabled, reference from other sensors if possible.
// Otherwise, add the smallest increment to the previous data frame time.
// 2) The newly enabled sensor could only underestimate its
// frame time without reference from other sensors.
// 3) The underestimated frame time of a newly enabled sensor will be corrected
// as soon as it shows up in the same frame with another sensor.
// 4) (prev_frame_time == ULONG_LONG_MAX) means the sensor wasn't enabled.
// 5) (prev_frame_time == ULONG_LONG_MAX -1) means the sensor didn't appear in the last
// data frame of the previous fifo read. So it won't be used as a frame time reference.
tmp_frame_time = 0;
for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) {
observed[j] = false; // reset at each data frame
tmp_time[j] = 0;
if ((mTask.prev_frame_time[j] < ULONG_LONG_MAX - 1) && (fh_param & (1 << j))) {
tmp_time[j] = mTask.prev_frame_time[j] + mTask.time_delta[j];
tmp_frame_time = (tmp_time[j] > tmp_frame_time) ? tmp_time[j] : tmp_frame_time;
}
}
tmp_frame_time = (frame_sensor_time + kMinSensorTimeIncrement > tmp_frame_time)
? (frame_sensor_time + kMinSensorTimeIncrement) : tmp_frame_time;
// regular frame, dispatch data to each sensor's own fifo
#ifdef MAG_SLAVE_PRESENT
if (fh_param & 4) { // have mag data
if (size >= 8) {
if (frame_sensor_time_valid) {
// scale not used
parseRawData(&mTask.sensors[MAG], &buf[i], 0, tmp_frame_time);
#if TIMESTAMP_DBG
if (mTask.prev_frame_time[MAG] == ULONG_LONG_MAX) {
DEBUG_PRINT("mag enabled: frame %d time 0x%08x\n",
frame_num, (unsigned int)tmp_frame_time);
} else if ((tmp_frame_time != tmp_time[MAG]) && (tmp_time[MAG] != 0)) {
DEBUG_PRINT("frame %d mag time: 0x%08x -> 0x%08x, jumped %d msec\n",
frame_num,
(unsigned int)tmp_time[MAG],
(unsigned int)tmp_frame_time,
(int)(5 * ((int64_t)(tmp_frame_time - tmp_time[MAG]) >> 7)));
}
#endif
}
mTask.prev_frame_time[MAG] = tmp_frame_time;
i += 8;
size -= 8;
observed[MAG] = true;
} else {
size = 0;
}
}
#endif
if (fh_param & 2) { // have gyro data
if (size >= 6) {
if (frame_sensor_time_valid) {
parseRawData(&mTask.sensors[GYR], &buf[i], kScale_gyr, tmp_frame_time);
#if TIMESTAMP_DBG
if (mTask.prev_frame_time[GYR] == ULONG_LONG_MAX) {
DEBUG_PRINT("gyr enabled: frame %d time 0x%08x\n",
frame_num, (unsigned int)tmp_frame_time);
} else if ((tmp_frame_time != tmp_time[GYR]) && (tmp_time[GYR] != 0)) {
DEBUG_PRINT("frame %d gyr time: 0x%08x -> 0x%08x, jumped %d msec\n",
frame_num,
(unsigned int)tmp_time[GYR],
(unsigned int)tmp_frame_time,
(int)(5 * ((int64_t)(tmp_frame_time - tmp_time[GYR]) >> 7)));
}
#endif
}
mTask.prev_frame_time[GYR] = tmp_frame_time;
i += 6;
size -= 6;
observed[GYR] = true;
} else {
size = 0;
}
}
if (fh_param & 1) { // have accel data
if (size >= 6) {
if (frame_sensor_time_valid) {
parseRawData(&mTask.sensors[ACC], &buf[i], kScale_acc, tmp_frame_time);
#if TIMESTAMP_DBG
if (mTask.prev_frame_time[ACC] == ULONG_LONG_MAX) {
DEBUG_PRINT("acc enabled: frame %d time 0x%08x\n",
frame_num, (unsigned int)tmp_frame_time);
} else if ((tmp_frame_time != tmp_time[ACC]) && (tmp_time[ACC] != 0)) {
DEBUG_PRINT("frame %d gyr time: 0x%08x -> 0x%08x, jumped %d msec\n",
frame_num,
(unsigned int)tmp_time[ACC],
(unsigned int)tmp_frame_time,
(int)(5 * ((int64_t)(tmp_frame_time - tmp_time[ACC]) >> 7)));
}
#endif
}
mTask.prev_frame_time[ACC] = tmp_frame_time;
i += 6;
size -= 6;
observed[ACC] = true;
} else {
size = 0;
}
}
if (observed[ACC] || observed[GYR])
frame_sensor_time = tmp_frame_time;
#ifdef MAG_SLAVE_PRESENT
else if (observed[MAG])
frame_sensor_time = tmp_frame_time;
#endif
} else {
size = 0; // drop this batch
ERROR_PRINT("Invalid fh_mode\n");
}
}
//flush data events.
flushAllData();
}
/*
* Read the interrupt type and send corresponding event
* If it's anymo or double tap, also send a single uint32 to indicate which axies
* is this interrupt triggered.
* If it's flat, also send a bit to indicate flat/non-flat position.
* If it's step detector, check if we need to send the total step count.
*/
static void int2Handling(void)
{
TDECL();
union EmbeddedDataPoint trigger_axies;
uint8_t int_status_0 = mTask.statusBuffer[1];
uint8_t int_status_1 = mTask.statusBuffer[2];
if (int_status_0 & INT_STEP) {
if (mTask.sensors[STEP].powered) {
DEBUG_PRINT("Detected step\n");
osEnqueueEvt(EVT_SENSOR_STEP, NULL, NULL);
}
if (mTask.sensors[STEPCNT].powered) {
T(pending_step_cnt) = true;
}
}
if ((int_status_0 & INT_ANY_MOTION) && mTask.sensors[ANYMO].powered) {
// bit [0:2] of INT_STATUS[2] is set when anymo is triggered by x, y or
// z axies respectively. bit [3] indicates the slope.
trigger_axies.idata = (mTask.statusBuffer[3] & 0x0f);
DEBUG_PRINT("Detected any motion\n");
osEnqueueEvt(EVT_SENSOR_ANY_MOTION, trigger_axies.vptr, NULL);
}
if ((int_status_0 & INT_DOUBLE_TAP) && mTask.sensors[DTAP].powered) {
// bit [4:6] of INT_STATUS[2] is set when double tap is triggered by
// x, y or z axies respectively. bit [7] indicates the slope.
trigger_axies.idata = ((mTask.statusBuffer[3] & 0xf0) >> 4);
DEBUG_PRINT("Detected double tap\n");
osEnqueueEvt(EVT_SENSOR_DOUBLE_TAP, trigger_axies.vptr, NULL);
}
if ((int_status_0 & INT_FLAT) && mTask.sensors[FLAT].powered) {
// bit [7] of INT_STATUS[3] indicates flat/non-flat position
trigger_axies.idata = ((mTask.statusBuffer[4] & 0x80) >> 7);
DEBUG_PRINT("Detected flat\n");
osEnqueueEvt(EVT_SENSOR_FLAT, trigger_axies.vptr, NULL);
}
if ((int_status_1 & INT_NO_MOTION) && mTask.sensors[NOMO].powered) {
DEBUG_PRINT("Detected no motion\n");
osEnqueueEvt(EVT_SENSOR_NO_MOTION, NULL, NULL);
}
return;
}
static void int2Evt(void)
{
TDECL();
if (trySwitchState(SENSOR_INT_2_HANDLING)) {
// Read the interrupt reg value to determine what interrupts
SPI_READ(BMI160_REG_INT_STATUS_0, 4, &mTask.statusBuffer);
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, __FUNCTION__);
} else {
// even if we are still in SENSOR_INT_2_HANDLING, the SPI may already finished and we need
// to issue another SPI read to get the latest status
mTask.pending_int[1] = true;
}
}
// bits[6:7] in OFFSET[6] to enable/disable gyro/accel offset.
// bits[0:5] in OFFSET[6] stores the most significant 2 bits of gyro offset at
// its x, y, z axies.
// Calculate the stored gyro offset and compose it with the intended
// enable/disable mode for gyro/accel offset to determine the value for
// OFFSET[6].
static uint8_t offset6Mode(void)
{
uint8_t mode = 0;
if (mTask.sensors[GYR].offset_enable)
mode |= 0x01 << 7;
if (mTask.sensors[ACC].offset_enable)
mode |= 0x01 << 6;
mode |= (mTask.sensors[GYR].offset[2] & 0x0300) >> 4;
mode |= (mTask.sensors[GYR].offset[1] & 0x0300) >> 6;
mode |= (mTask.sensors[GYR].offset[0] & 0x0300) >> 8;
DEBUG_PRINT("OFFSET_6_MODE is: %02x\n", mode);
return mode;
}
static bool saveCalibration()
{
TDECL();
if (trySwitchState(SENSOR_SAVE_CALIBRATION)) {
if (mTask.sensors[ACC].offset_enable) {
SPI_WRITE(BMI160_REG_OFFSET_0, mTask.sensors[ACC].offset[0] & 0xFF, 450);
SPI_WRITE(BMI160_REG_OFFSET_0 + 1, mTask.sensors[ACC].offset[1] & 0xFF, 450);
SPI_WRITE(BMI160_REG_OFFSET_0 + 2, mTask.sensors[ACC].offset[2] & 0xFF, 450);
}
if (mTask.sensors[GYR].offset_enable) {
SPI_WRITE(BMI160_REG_OFFSET_3, mTask.sensors[GYR].offset[0] & 0xFF, 450);
SPI_WRITE(BMI160_REG_OFFSET_3 + 1, mTask.sensors[GYR].offset[1] & 0xFF, 450);
SPI_WRITE(BMI160_REG_OFFSET_3 + 2, mTask.sensors[GYR].offset[2] & 0xFF, 450);
}
SPI_WRITE(BMI160_REG_OFFSET_6, offset6Mode(), 450);
SPI_READ(BMI160_REG_OFFSET_0, 7, &mTask.dataBuffer);
spiBatchTxRx(&mTask.mode, sensorSpiCallback, NULL, __FUNCTION__);
return true;
} else {
DEBUG_PRINT("%s, state != IDLE", __FUNCTION__);
return false;
}
}
static void sendCalibrationResult(uint8_t status, uint8_t sensorType,
int32_t xBias, int32_t yBias, int32_t zBias) {
struct CalibrationData *data = heapAlloc(sizeof(struct CalibrationData));
if (!data) {
osLog(LOG_WARN, "Couldn't alloc cal result pkt");
return;
}
data->header.appId = BMI160_APP_ID;
data->header.dataLen = (sizeof(struct CalibrationData) - sizeof(struct HostHubRawPacket));
data->data_header.msgId = SENSOR_APP_MSG_ID_CAL_RESULT;
data->data_header.sensorType = sensorType;
data->data_header.status = status;
data->xBias = xBias;
data->yBias = yBias;
data->zBias = zBias;
if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree))
osLog(LOG_WARN, "Couldn't send cal result evt");
}
static void accCalibrationHandling(void)
{
TDECL();
switch (mTask.calibration_state) {
case CALIBRATION_START:
T(mRetryLeft) = RETRY_CNT_CALIBRATION;
// turn ACC to NORMAL mode
SPI_WRITE(BMI160_REG_CMD, 0x11, 50000);
mTask.calibration_state = CALIBRATION_FOC;
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
break;
case CALIBRATION_FOC:
// set accel range to +-8g
SPI_WRITE(BMI160_REG_ACC_RANGE, 0x08);
// enable accel fast offset compensation,
// x: 0g, y: 0g, z: 1g
SPI_WRITE(BMI160_REG_FOC_CONF, ACC_FOC_CONFIG);
// start calibration
SPI_WRITE(BMI160_REG_CMD, 0x03, 100000);
// poll the status reg until the calibration finishes.
SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000);
mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE;
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
break;
case CALIBRATION_WAIT_FOC_DONE:
// if the STATUS REG has bit 3 set, it means calbration is done.
// otherwise, check back in 50ms later.
if (mTask.statusBuffer[1] & 0x08) {
//disable FOC
SPI_WRITE(BMI160_REG_FOC_CONF, 0x00);
//read the offset value for accel
SPI_READ(BMI160_REG_OFFSET_0, 3, &mTask.dataBuffer);
mTask.calibration_state = CALIBRATION_SET_OFFSET;
DEBUG_PRINT("FOC set FINISHED!\n");
} else {
// calibration hasn't finished yet, go back to wait for 50ms.
SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000);
mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE;
T(mRetryLeft)--;
}
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
// if calbration hasn't finished after 10 polling on the STATUS reg,
// declare timeout.
if (T(mRetryLeft) == 0) {
mTask.calibration_state = CALIBRATION_TIMEOUT;
}
break;
case CALIBRATION_SET_OFFSET:
mTask.sensors[ACC].offset[0] = mTask.dataBuffer[1];
mTask.sensors[ACC].offset[1] = mTask.dataBuffer[2];
mTask.sensors[ACC].offset[2] = mTask.dataBuffer[3];
// sign extend values
if (mTask.sensors[ACC].offset[0] & 0x80)
mTask.sensors[ACC].offset[0] |= 0xFFFFFF00;
if (mTask.sensors[ACC].offset[1] & 0x80)
mTask.sensors[ACC].offset[1] |= 0xFFFFFF00;
if (mTask.sensors[ACC].offset[2] & 0x80)
mTask.sensors[ACC].offset[2] |= 0xFFFFFF00;
mTask.sensors[ACC].offset_enable = true;
DEBUG_PRINT("ACCELERATION OFFSET is %02x %02x %02x\n",
(unsigned int)mTask.sensors[ACC].offset[0],
(unsigned int)mTask.sensors[ACC].offset[1],
(unsigned int)mTask.sensors[ACC].offset[2]);
sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_ACCEL,
mTask.sensors[ACC].offset[0], mTask.sensors[ACC].offset[1],
mTask.sensors[ACC].offset[2]);
// Enable offset compensation for accel
uint8_t mode = offset6Mode();
SPI_WRITE(BMI160_REG_OFFSET_6, mode);
// turn ACC to SUSPEND mode
SPI_WRITE(BMI160_REG_CMD, 0x10, 5000);
mTask.calibration_state = CALIBRATION_DONE;
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
break;
default:
ERROR_PRINT("Invalid calibration state\n");
break;
}
}
static bool accCalibration(void *cookie)
{
TDECL();
if (!mTask.sensors[ACC].powered && trySwitchState(SENSOR_CALIBRATING)) {
mTask.calibration_state = CALIBRATION_START;
accCalibrationHandling();
return true;
} else {
ERROR_PRINT("cannot calibrate accel because sensor is busy\n");
sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_ACCEL, 0, 0, 0);
return false;
}
}
static bool accCfgData(void *data, void *cookie)
{
int32_t *values = data;
mTask.sensors[ACC].offset[0] = values[0];
mTask.sensors[ACC].offset[1] = values[1];
mTask.sensors[ACC].offset[2] = values[2];
mTask.sensors[ACC].offset_enable = true;
INFO_PRINT("accCfgData: data=%02lx, %02lx, %02lx\n",
values[0] & 0xFF, values[1] & 0xFF, values[2] & 0xFF);
if (!saveCalibration()) {
mTask.pending_calibration_save = true;
}
return true;
}
static void sendTestResult(uint8_t status, uint8_t sensorType) {
struct TestResultData *data = heapAlloc(sizeof(struct TestResultData));
if (!data) {
osLog(LOG_WARN, "Couldn't alloc test result packet");
return;
}
data->header.appId = BMI160_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))
osLog(LOG_WARN, "Couldn't send test result packet");
}
static void accTestHandling(void)
{
// the minimum absolute differences, according to BMI160 datasheet section
// 2.8.1, are 800 mg for the x and y axes and 400 mg for the z axis
static const int32_t kMinDifferenceXY = (800 * 32767) / 8000;
static const int32_t kMinDifferenceZ = (400 * 32767) / 8000;
int32_t tempTestX, tempTestY, tempTestZ;
int32_t absDiffX, absDiffY, absDiffZ;
TDECL();
switch (mTask.acc_test_state) {
case ACC_TEST_START:
// turn ACC to NORMAL mode
SPI_WRITE(BMI160_REG_CMD, 0x11, 50000);
mTask.acc_test_state = ACC_TEST_CONFIG;
spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
break;
case ACC_TEST_CONFIG:
// set accel conf
SPI_WRITE(BMI160_REG_ACC_CONF, 0x2c);
// set accel range to +-8g
SPI_WRITE(BMI160_REG_ACC_RANGE, 0x08);
// read stale accel data
SPI_READ(BMI160_REG_DATA_14, 6, &mTask.dataBuffer);
mTask