mirror of
https://github.com/google/pebble.git
synced 2025-06-05 01:33:11 +00:00
Import of the watch repository from Pebble
This commit is contained in:
commit
3b92768480
10334 changed files with 2564465 additions and 0 deletions
898
src/fw/drivers/imu/bma255/bma255.c
Normal file
898
src/fw/drivers/imu/bma255/bma255.c
Normal file
|
@ -0,0 +1,898 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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 "bma255.h"
|
||||
#include "bma255_regs.h"
|
||||
#include "bma255_private.h"
|
||||
|
||||
#include "console/prompt.h"
|
||||
#include "drivers/accel.h"
|
||||
#include "drivers/exti.h"
|
||||
#include "drivers/gpio.h"
|
||||
#include "drivers/periph_config.h"
|
||||
#include "drivers/rtc.h"
|
||||
#include "drivers/spi.h"
|
||||
#include "kernel/util/delay.h"
|
||||
#include "kernel/util/sleep.h"
|
||||
#include "system/logging.h"
|
||||
#include "system/passert.h"
|
||||
#include "util/math.h"
|
||||
#include "util/units.h"
|
||||
|
||||
#define STM32F2_COMPATIBLE
|
||||
#define STM32F4_COMPATIBLE
|
||||
#include <mcu.h>
|
||||
|
||||
#define BMA255_DEBUG 0
|
||||
|
||||
#if BMA255_DEBUG
|
||||
#define BMA255_DBG(msg, ...) \
|
||||
do { \
|
||||
PBL_LOG(LOG_LEVEL_DEBUG, msg, __VA_ARGS__); \
|
||||
} while (0);
|
||||
#else
|
||||
#define BMA255_DBG(msg, ...)
|
||||
#endif
|
||||
|
||||
|
||||
#define SELFTEST_SIGN_POSITIVE (0x1 << 2)
|
||||
#define SELFTEST_SIGN_NEGATIVE (0x0)
|
||||
|
||||
// The BMA255 is capable of storing up to 32 frames.
|
||||
// Conceptually each frame consists of three 16-bit words corresponding to the x, y and z- axis.
|
||||
#define BMA255_FIFO_MAX_FRAMES (32)
|
||||
#define BMA255_FIFO_FRAME_SIZE_BYTES (3 * 2)
|
||||
#define BMA255_FIFO_SIZE_BYTES (BMA255_FIFO_MAX_FRAMES * BMA255_FIFO_FRAME_SIZE_BYTES)
|
||||
|
||||
|
||||
// Driver state
|
||||
static BMA255PowerMode s_accel_power_mode = BMA255PowerMode_Normal;
|
||||
static bool s_fifo_is_enabled = false;
|
||||
static bool s_shake_detection_enabled = false;
|
||||
static bool s_accel_outstanding_motion_work = false;
|
||||
static bool s_accel_outstanding_data_work = false;
|
||||
static bool s_fifo_overrun_detected = false;
|
||||
|
||||
|
||||
// Forward declarations
|
||||
static void prv_configure_operating_mode(void);
|
||||
static void prv_bma255_IRQ1_handler(bool *should_context_switch);
|
||||
static void prv_bma255_IRQ2_handler(bool *should_context_switch);
|
||||
static void prv_set_accel_power_mode(BMA255PowerMode mode);
|
||||
|
||||
|
||||
// The BMA255 reports each G in powers of 2 with full deflection at +-2^11
|
||||
// So scale all readings by (scale)/(2^11) to get G
|
||||
// And scale the result by 1000 to allow for easier interger math
|
||||
typedef enum {
|
||||
BMA255Scale_2G = 980, // 2000/2048
|
||||
BMA255Scale_4G = 1953, // 4000/2048
|
||||
BMA255Scale_8G = 3906, // 8000/2048
|
||||
BMA255Scale_16G = 7813, // 16000/2048
|
||||
} BMA255Scale;
|
||||
|
||||
static int16_t s_raw_unit_to_mgs = BMA255Scale_2G;
|
||||
|
||||
typedef enum {
|
||||
AccelOperatingMode_Data,
|
||||
AccelOperatingMode_ShakeDetection,
|
||||
AccelOperatingMode_DoubleTapDetection,
|
||||
|
||||
AccelOperatingModeCount,
|
||||
} AccelOperatingMode;
|
||||
|
||||
static struct {
|
||||
bool enabled;
|
||||
bool using_interrupts;
|
||||
BMA255ODR odr;
|
||||
} s_operating_states[AccelOperatingModeCount] = {
|
||||
[AccelOperatingMode_Data] = {
|
||||
.enabled = false,
|
||||
.using_interrupts = false,
|
||||
.odr = BMA255ODR_125HZ,
|
||||
},
|
||||
[AccelOperatingMode_ShakeDetection] = {
|
||||
.enabled = false,
|
||||
.using_interrupts = false,
|
||||
.odr = BMA255ODR_125HZ,
|
||||
},
|
||||
[AccelOperatingMode_DoubleTapDetection] = {
|
||||
.enabled = false,
|
||||
.using_interrupts = false,
|
||||
.odr = BMA255ODR_125HZ,
|
||||
},
|
||||
};
|
||||
|
||||
void bma255_init(void) {
|
||||
bma255_gpio_init();
|
||||
if (!bma255_query_whoami()) {
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Failed to query BMA255");
|
||||
return;
|
||||
}
|
||||
const bool pass = bma255_selftest();
|
||||
if (pass) {
|
||||
PBL_LOG(LOG_LEVEL_DEBUG, "BMA255 self test pass, all 3 axis");
|
||||
} else {
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "BMA255 self test failed one or more axis");
|
||||
}
|
||||
|
||||
// Workaround to fix FIFO Frame Leakage: Disable temperature sensor (we're not using it anyways)
|
||||
// See Section 2.2.1 of https://drive.google.com/a/pebble.com/file/d/0B9tTN3OlYns3bEZaczdoZUU3UEk/view
|
||||
bma255_write_register(BMA255Register_EXTENDED_MEMORY_MAP, BMA255_EXTENDED_MEMORY_MAP_OPEN);
|
||||
bma255_write_register(BMA255Register_EXTENDED_MEMORY_MAP, BMA255_EXTENDED_MEMORY_MAP_OPEN);
|
||||
bma255_write_register(BMA255Register_TEMPERATURE_SENSOR_CTRL, BMA255_TEMPERATURE_SENSOR_DISABLE);
|
||||
bma255_write_register(BMA255Register_EXTENDED_MEMORY_MAP, BMA255_EXTENDED_MEMORY_MAP_CLOSE);
|
||||
|
||||
exti_configure_pin(BOARD_CONFIG_ACCEL.accel_ints[0], ExtiTrigger_Rising, prv_bma255_IRQ1_handler);
|
||||
exti_configure_pin(BOARD_CONFIG_ACCEL.accel_ints[1], ExtiTrigger_Rising, prv_bma255_IRQ2_handler);
|
||||
}
|
||||
|
||||
bool bma255_query_whoami(void) {
|
||||
const uint8_t chip_id = bma255_read_register(BMA255Register_BGW_CHIP_ID);
|
||||
PBL_LOG(LOG_LEVEL_DEBUG, "Read BMA255 whoami byte 0x%"PRIx8", expecting 0x%"PRIx8,
|
||||
chip_id, BMA255_CHIP_ID);
|
||||
return (chip_id == BMA255_CHIP_ID);
|
||||
}
|
||||
|
||||
static uint64_t prv_get_curr_system_time_ms(void) {
|
||||
time_t time_s;
|
||||
uint16_t time_ms;
|
||||
rtc_get_time_ms(&time_s, &time_ms);
|
||||
return (((uint64_t)time_s) * 1000 + time_ms);
|
||||
}
|
||||
|
||||
void bma255_set_scale(BMA255Scale scale) {
|
||||
uint8_t value = 0;
|
||||
switch (scale) {
|
||||
case BMA255Scale_2G:
|
||||
value = 0x3;
|
||||
break;
|
||||
case BMA255Scale_4G:
|
||||
value = 0x5;
|
||||
break;
|
||||
case BMA255Scale_8G:
|
||||
value = 0x8;
|
||||
break;
|
||||
case BMA255Scale_16G:
|
||||
value = 0xc;
|
||||
break;
|
||||
default:
|
||||
WTF;
|
||||
}
|
||||
bma255_write_register(BMA255Register_PMU_RANGE, value);
|
||||
s_raw_unit_to_mgs = scale;
|
||||
}
|
||||
|
||||
static int16_t prv_raw_to_mgs(int16_t raw_val) {
|
||||
int16_t mgs = ((int32_t)raw_val * s_raw_unit_to_mgs) / 1000;
|
||||
return mgs;
|
||||
}
|
||||
|
||||
static int16_t prv_conv_raw_to_12bit(const uint8_t registers[2]) {
|
||||
int16_t reading = ((registers[0] >> 4) & 0x0F) | ((int16_t)registers[1] << 4);
|
||||
if (reading & 0x0800) {
|
||||
reading |= 0xF000;
|
||||
}
|
||||
return reading;
|
||||
}
|
||||
|
||||
static void prv_convert_accel_raw_data_to_mgs(const uint8_t *buf, AccelDriverSample *data) {
|
||||
int16_t readings[3];
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
readings[i] = prv_conv_raw_to_12bit(&buf[i*2]);
|
||||
}
|
||||
const AccelConfig *cfg = &BOARD_CONFIG_ACCEL.accel_config;
|
||||
*data = (AccelDriverSample) {
|
||||
.x = (cfg->axes_inverts[AXIS_X] ? -1 : 1) *
|
||||
prv_raw_to_mgs(readings[cfg->axes_offsets[AXIS_X]]),
|
||||
.y = (cfg->axes_inverts[AXIS_Y] ? -1 : 1) *
|
||||
prv_raw_to_mgs(readings[cfg->axes_offsets[AXIS_Y]]),
|
||||
.z = (cfg->axes_inverts[AXIS_Z] ? -1 : 1) *
|
||||
prv_raw_to_mgs(readings[cfg->axes_offsets[AXIS_Z]]),
|
||||
};
|
||||
}
|
||||
|
||||
static void prv_read_curr_accel_data(AccelDriverSample *data) {
|
||||
uint8_t raw_buf[BMA255_FIFO_FRAME_SIZE_BYTES];
|
||||
bma255_burst_read(BMA255Register_ACCD_X_LSB, raw_buf, sizeof(raw_buf));
|
||||
|
||||
prv_convert_accel_raw_data_to_mgs(raw_buf, data);
|
||||
// FIXME: assuming the timestamp on the samples is NOW! PBL-33765
|
||||
data->timestamp_us = prv_get_curr_system_time_ms() * 1000;
|
||||
|
||||
BMA255_DBG("%"PRId16" %"PRId16" %"PRId16, data->x, data->y, data->z);
|
||||
}
|
||||
|
||||
typedef enum {
|
||||
BMA255Axis_X = 0,
|
||||
BMA255Axis_Y,
|
||||
BMA255Axis_Z,
|
||||
} BMA255Axis;
|
||||
|
||||
static void prv_drain_fifo(void) {
|
||||
// TODO: I think the ideal thing to do here would be to invoke the accel_cb_new_sample() while
|
||||
// the SPI transaction is in progress so we don't need a static ~500 byte buffer. (This is what
|
||||
// we do in the bmi160 driver) However, since we are oversampling super aggressively with the
|
||||
// bma255, I'm concerned about changing the timing of how fast we drain things. Thus, just use a
|
||||
// static buffer for now. This should be safe because only one thread should be draining the data.
|
||||
static AccelDriverSample data[BMA255_FIFO_MAX_FRAMES];
|
||||
const uint64_t timestamp_us = prv_get_curr_system_time_ms() * 1000;
|
||||
const uint32_t sampling_interval_us = accel_get_sampling_interval();
|
||||
|
||||
uint8_t fifo_status = bma255_read_register(BMA255Register_FIFO_STATUS);
|
||||
BMA255_DBG("Drain %"PRIu8" samples", num_samples_available);
|
||||
|
||||
const uint8_t num_samples_available = fifo_status & 0x3f;
|
||||
if (num_samples_available == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
bma255_prepare_txn(BMA255Register_FIFO_DATA | BMA255_READ_FLAG);
|
||||
for (int i = 0; i < num_samples_available; ++i) {
|
||||
uint8_t buf[BMA255_FIFO_FRAME_SIZE_BYTES];
|
||||
for (int j = 0; j < BMA255_FIFO_FRAME_SIZE_BYTES; ++j) {
|
||||
buf[j] = bma255_send_and_receive_byte(0);
|
||||
}
|
||||
prv_convert_accel_raw_data_to_mgs(buf, &data[i]);
|
||||
}
|
||||
bma255_end_txn();
|
||||
|
||||
// Timestamp & Dispatch data
|
||||
for (int i = 0; i < num_samples_available; ++i) {
|
||||
// Make a timestamp approximation based on the current time, the sample
|
||||
// being processed and the sampling interval.
|
||||
data[i].timestamp_us = timestamp_us - ((num_samples_available - i) * sampling_interval_us);
|
||||
BMA255_DBG("%2d: %"PRId16" %"PRId16" %"PRId16" %"PRIu32,
|
||||
i, data[i].x, data[i].y, data[i].z, (uint32_t)data[i].timestamp_us);
|
||||
accel_cb_new_sample(&data[i]);
|
||||
}
|
||||
|
||||
// clear of fifo overrun flag must happen after draining samples, also the samples available will
|
||||
// get drained too!
|
||||
if ((fifo_status & 0x80) && !s_fifo_overrun_detected) {
|
||||
s_fifo_overrun_detected = true;
|
||||
// We don't clear the interrupt here because you are only supposed to touch the fifo config
|
||||
// registers while in standby mode.
|
||||
PBL_LOG(LOG_LEVEL_WARNING, "bma255 fifo overrun detected: 0x%x!", fifo_status);
|
||||
}
|
||||
}
|
||||
|
||||
static void prv_handle_data(void) {
|
||||
s_accel_outstanding_data_work = false;
|
||||
if (s_fifo_is_enabled) {
|
||||
prv_drain_fifo();
|
||||
return;
|
||||
}
|
||||
|
||||
AccelDriverSample data;
|
||||
prv_read_curr_accel_data(&data);
|
||||
accel_cb_new_sample(&data);
|
||||
}
|
||||
|
||||
static void prv_handle_motion_interrupts(void) {
|
||||
s_accel_outstanding_motion_work = false;
|
||||
|
||||
const uint8_t int0_status = bma255_read_register(BMA255Register_INT_STATUS_0);
|
||||
const uint8_t int2_status = bma255_read_register(BMA255Register_INT_STATUS_2);
|
||||
|
||||
bool anymotion = (int0_status & BMA255_INT_STATUS_0_SLOPE_MASK);
|
||||
if (anymotion) {
|
||||
const AccelConfig *cfg = &BOARD_CONFIG_ACCEL.accel_config;
|
||||
IMUCoordinateAxis axis = AXIS_X;
|
||||
bool invert = false;
|
||||
|
||||
if (int2_status & BMA255_INT_STATUS_2_SLOPE_FIRST_X) {
|
||||
axis = AXIS_X;
|
||||
invert = cfg->axes_inverts[AXIS_X];
|
||||
} else if (int2_status & BMA255_INT_STATUS_2_SLOPE_FIRST_Y) {
|
||||
axis = AXIS_Y;
|
||||
invert = cfg->axes_inverts[AXIS_Y];
|
||||
} else if (int2_status & BMA255_INT_STATUS_2_SLOPE_FIRST_Z) {
|
||||
axis = AXIS_Z;
|
||||
invert = cfg->axes_inverts[AXIS_Z];
|
||||
} else {
|
||||
BMA255_DBG("No Axis?: 0x%"PRIx8" 0x%"PRIx8, int0_status, int2_status);
|
||||
}
|
||||
int32_t direction = ((int2_status & BMA255_INT_STATUS_2_SLOPE_SIGN) == 0) ? 1 : -1;
|
||||
direction *= (invert ? -1 : 1);
|
||||
|
||||
accel_cb_shake_detected(axis, direction);
|
||||
}
|
||||
}
|
||||
|
||||
// Services tap/motion interrupts
|
||||
static void prv_bma255_IRQ1_handler(bool *should_context_switch) {
|
||||
BMA255_DBG("Slope Int");
|
||||
if (!s_accel_outstanding_motion_work) {
|
||||
s_accel_outstanding_motion_work = true;
|
||||
accel_offload_work_from_isr(prv_handle_motion_interrupts, should_context_switch);
|
||||
}
|
||||
}
|
||||
|
||||
// Services data / fifo interrupts.
|
||||
// NOTE: The BMA255 Errata specifically states that we should keep the fifo /
|
||||
// data interrupt on INT2 to avoid "data inconsistencies" which arise when
|
||||
// we have it fifo / data interrupt on INT1.
|
||||
static void prv_bma255_IRQ2_handler(bool *should_context_switch) {
|
||||
BMA255_DBG("Data Int");
|
||||
if (!s_accel_outstanding_data_work) {
|
||||
s_accel_outstanding_data_work = true;
|
||||
accel_offload_work_from_isr(prv_handle_data, should_context_switch);
|
||||
}
|
||||
}
|
||||
|
||||
static void prv_update_accel_interrupts(bool enable, AccelOperatingMode mode) {
|
||||
s_operating_states[mode].using_interrupts = enable;
|
||||
|
||||
bool enable_interrupts = false;
|
||||
for (uint32_t i = 0; i < ARRAY_LENGTH(s_operating_states); i++) {
|
||||
if (s_operating_states[i].using_interrupts) {
|
||||
enable_interrupts = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (enable_interrupts) {
|
||||
exti_enable(BOARD_CONFIG_ACCEL.accel_ints[0]);
|
||||
exti_enable(BOARD_CONFIG_ACCEL.accel_ints[1]);
|
||||
} else {
|
||||
exti_disable(BOARD_CONFIG_ACCEL.accel_ints[0]);
|
||||
exti_disable(BOARD_CONFIG_ACCEL.accel_ints[1]);
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t accel_get_sampling_interval(void) {
|
||||
BMA255ODR odr_max = 0;
|
||||
for (uint32_t i = 0; i < ARRAY_LENGTH(s_operating_states); i++) {
|
||||
if (s_operating_states[i].enabled) {
|
||||
odr_max = MAX(odr_max, s_operating_states[i].odr);
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t sample_interval = 0;
|
||||
switch (odr_max) {
|
||||
case BMA255ODR_1HZ:
|
||||
sample_interval = BMA255SampleInterval_1HZ;
|
||||
break;
|
||||
case BMA255ODR_10HZ:
|
||||
sample_interval = BMA255SampleInterval_10HZ;
|
||||
break;
|
||||
case BMA255ODR_19HZ:
|
||||
sample_interval = BMA255SampleInterval_19HZ;
|
||||
break;
|
||||
case BMA255ODR_83HZ:
|
||||
sample_interval = BMA255SampleInterval_83HZ;
|
||||
break;
|
||||
case BMA255ODR_125HZ:
|
||||
sample_interval = BMA255SampleInterval_125HZ;
|
||||
break;
|
||||
case BMA255ODR_166HZ:
|
||||
sample_interval = BMA255SampleInterval_166HZ;
|
||||
break;
|
||||
case BMA255ODR_250HZ:
|
||||
sample_interval = BMA255SampleInterval_250HZ;
|
||||
break;
|
||||
default:
|
||||
WTF;
|
||||
}
|
||||
return sample_interval;
|
||||
}
|
||||
|
||||
//! Set the LOW_POWER and LPW registers as required.
|
||||
//! The LPW register is masked because it contains the sleep duration for our desired ODR.
|
||||
static void prv_enter_power_mode(BMA255PowerMode mode) {
|
||||
bma255_write_register(BMA255Register_PMU_LOW_POWER,
|
||||
s_power_mode[mode].low_power << BMA255_LOW_POWER_SHIFT);
|
||||
bma255_read_modify_write(BMA255Register_PMU_LPW,
|
||||
s_power_mode[mode].lpw << BMA255_LPW_POWER_SHIFT,
|
||||
BMA255_LPW_POWER_MASK);
|
||||
|
||||
// Workaround for error in transition to Suspend / Standby
|
||||
if (mode == BMA255PowerMode_Suspend || mode == BMA255PowerMode_Standby) {
|
||||
// Write to FIFO_CONFIG_1 to exit some unknown "intermittent state"
|
||||
// NOTE: This will clear the FIFO & FIFO status.
|
||||
bma255_read_modify_write(BMA255Register_FIFO_CONFIG_1, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
static void prv_set_accel_power_mode(BMA255PowerMode mode) {
|
||||
PBL_ASSERTN(mode == BMA255PowerMode_Normal ||
|
||||
mode == BMA255PowerMode_LowPower1 ||
|
||||
mode == BMA255PowerMode_Standby);
|
||||
|
||||
// Workaround for entering Normal Mode
|
||||
// LPM1 => Normal requires us to go through Suspend mode
|
||||
// LPM2 => Normal requires us to go through Standby mode
|
||||
if (mode == BMA255PowerMode_Normal) {
|
||||
if (s_accel_power_mode == BMA255PowerMode_LowPower1) {
|
||||
prv_enter_power_mode(BMA255PowerMode_Suspend);
|
||||
} else if (s_accel_power_mode == BMA255PowerMode_LowPower2) {
|
||||
prv_enter_power_mode(BMA255PowerMode_Standby);
|
||||
}
|
||||
}
|
||||
|
||||
prv_enter_power_mode(mode);
|
||||
|
||||
BMA255_DBG("BMA555: power level set to: 0x%x and 0x%x",
|
||||
bma255_read_register(BMA255Register_PMU_LPW),
|
||||
bma255_read_register(BMA255Register_PMU_LOW_POWER));
|
||||
|
||||
s_accel_power_mode = mode;
|
||||
}
|
||||
|
||||
static BMA255ODR prv_get_odr(BMA255SampleInterval sample_interval) {
|
||||
BMA255ODR odr = 0;
|
||||
switch (sample_interval) {
|
||||
case BMA255SampleInterval_1HZ:
|
||||
odr = BMA255ODR_1HZ;
|
||||
break;
|
||||
case BMA255SampleInterval_10HZ:
|
||||
odr = BMA255ODR_10HZ;
|
||||
break;
|
||||
case BMA255SampleInterval_19HZ:
|
||||
odr = BMA255ODR_19HZ;
|
||||
break;
|
||||
case BMA255SampleInterval_83HZ:
|
||||
odr = BMA255ODR_83HZ;
|
||||
break;
|
||||
case BMA255SampleInterval_125HZ:
|
||||
odr = BMA255ODR_125HZ;
|
||||
break;
|
||||
case BMA255SampleInterval_166HZ:
|
||||
odr = BMA255ODR_166HZ;
|
||||
break;
|
||||
case BMA255SampleInterval_250HZ:
|
||||
odr = BMA255ODR_250HZ;
|
||||
break;
|
||||
default:
|
||||
WTF;
|
||||
}
|
||||
return odr;
|
||||
}
|
||||
|
||||
static BMA255SampleInterval prv_get_supported_sampling_interval(uint32_t interval_us) {
|
||||
BMA255SampleInterval sample_interval;
|
||||
if (BMA255SampleInterval_1HZ <= interval_us) {
|
||||
sample_interval = BMA255SampleInterval_1HZ;
|
||||
} else if (BMA255SampleInterval_10HZ <= interval_us) {
|
||||
sample_interval = BMA255SampleInterval_10HZ;
|
||||
} else if (BMA255SampleInterval_19HZ <= interval_us) {
|
||||
sample_interval = BMA255SampleInterval_19HZ;
|
||||
} else if (BMA255SampleInterval_83HZ <= interval_us) {
|
||||
sample_interval = BMA255SampleInterval_83HZ;
|
||||
} else if (BMA255SampleInterval_125HZ <= interval_us) {
|
||||
sample_interval = BMA255SampleInterval_125HZ;
|
||||
} else if (BMA255SampleInterval_166HZ <= interval_us) {
|
||||
sample_interval = BMA255SampleInterval_166HZ;
|
||||
} else if (BMA255SampleInterval_250HZ <= interval_us) {
|
||||
sample_interval = BMA255SampleInterval_250HZ;
|
||||
} else {
|
||||
sample_interval = BMA255SampleInterval_250HZ;
|
||||
}
|
||||
return sample_interval;
|
||||
}
|
||||
|
||||
static void prv_enable_operating_mode(AccelOperatingMode mode,
|
||||
BMA255SampleInterval sample_interval) {
|
||||
s_operating_states[mode].enabled = true;
|
||||
s_operating_states[mode].odr = prv_get_odr(sample_interval);
|
||||
prv_configure_operating_mode();
|
||||
}
|
||||
|
||||
static void prv_disable_operating_mode(AccelOperatingMode mode) {
|
||||
s_operating_states[mode].enabled = false;
|
||||
prv_configure_operating_mode();
|
||||
}
|
||||
|
||||
uint32_t accel_set_sampling_interval(uint32_t interval_us) {
|
||||
BMA255SampleInterval actual_interval = prv_get_supported_sampling_interval(interval_us);
|
||||
|
||||
// FIXME: For now, tie us to 125Hz. 125Hz is a rate that is easy enough to
|
||||
// subsample to all of our supported accel service rates, and also cuts down power consumption
|
||||
// from the 140uA range to 100uA.
|
||||
// Being able to sample at a lower rate like 38Hz will be able to get us down into the 40uA range.
|
||||
//
|
||||
// By forcing a sample interval of 125Hz here, we will never use a different
|
||||
// rate, and the accelerometer service will be made aware of our running rate.
|
||||
actual_interval = BMA255SampleInterval_125HZ;
|
||||
|
||||
prv_enable_operating_mode(AccelOperatingMode_Data, actual_interval);
|
||||
|
||||
return accel_get_sampling_interval();
|
||||
}
|
||||
|
||||
static void prv_configure_operating_mode(void) {
|
||||
BMA255SampleInterval interval_us = accel_get_sampling_interval();
|
||||
const uint8_t odr = (uint8_t)prv_get_odr(interval_us);
|
||||
const uint8_t bw = s_odr_settings[odr].bw;
|
||||
const uint8_t tsleep = s_odr_settings[odr].tsleep;
|
||||
|
||||
// Set the BW and TSleep to get the ODR we expect.
|
||||
bma255_write_register(BMA255Register_PMU_BW, bw);
|
||||
bma255_read_modify_write(BMA255Register_PMU_LPW,
|
||||
tsleep << BMA255_LPW_SLEEP_DUR_SHIFT,
|
||||
BMA255_LPW_SLEEP_DUR_MASK);
|
||||
|
||||
PBL_LOG(LOG_LEVEL_DEBUG, "Set sampling rate to %"PRIu32, (uint32_t)(1000000/interval_us));
|
||||
|
||||
if (s_accel_power_mode == BMA255PowerMode_Normal) {
|
||||
// This should only execute on startup or if the power mode
|
||||
// is left in normal power mode for some reason
|
||||
PBL_LOG(LOG_LEVEL_DEBUG, "Enable low power mode");
|
||||
prv_set_accel_power_mode(BMA255PowerMode_LowPower1);
|
||||
}
|
||||
}
|
||||
|
||||
int accel_peek(AccelDriverSample *data) {
|
||||
prv_read_curr_accel_data(data);
|
||||
return 0;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// FIFO Support
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
static void prv_program_fifo_register(uint8_t address, uint8_t data) {
|
||||
// To prevent lockups of the fifo, the fifo config registers should only be programmed
|
||||
// while in standby mode
|
||||
PBL_ASSERTN(s_accel_power_mode == BMA255PowerMode_Standby);
|
||||
const int retries = 2;
|
||||
uint8_t value;
|
||||
for (int i = 0; i <= retries; i++) {
|
||||
bma255_write_register(address, data);
|
||||
value = bma255_read_register(address);
|
||||
if (value == data) {
|
||||
return; // Write took, we are good
|
||||
}
|
||||
PBL_LOG(LOG_LEVEL_DEBUG, "FIFO config write failed, initiating workaround ...");
|
||||
|
||||
// FIXME: Sometimes writes to the FIFO registers fail. I am suspicious that the bma255 enters
|
||||
// suspend mode instead of standby mode. (The datasheet states that FIFO_CONFIG registers
|
||||
// accesses fail in suspend mode). It seems like the issue can be worked around by attempting
|
||||
// to enter standby mode again. Hopefully, bosch can illuminate for us what is going on here
|
||||
// but in the meantime let's use this workaround.
|
||||
prv_set_accel_power_mode(BMA255PowerMode_Normal);
|
||||
prv_set_accel_power_mode(BMA255PowerMode_Standby);
|
||||
}
|
||||
|
||||
PBL_LOG(LOG_LEVEL_WARNING, "Failed to program fifo reg, 0x%"PRIx8" = 0x%"PRIx8, address, data);
|
||||
}
|
||||
|
||||
static void prv_set_fifo_mode(BMA255FifoMode mode) {
|
||||
BMA255_DBG("Set Fifo Mode: 0x%x", mode);
|
||||
const uint8_t out =
|
||||
(mode << BMA255_FIFO_MODE_SHIFT) | (BMA255FifoDataSel_XYZ << BMA255_FIFO_DATA_SEL_SHIFT);
|
||||
prv_program_fifo_register(BMA255Register_FIFO_CONFIG_1, out);
|
||||
// If the fifo had overflowed, the write above will have cleared the flag
|
||||
s_fifo_overrun_detected = false;
|
||||
}
|
||||
|
||||
static void prv_configure_fifo_interrupts(bool enable_int, bool use_fifo) {
|
||||
BMA255_DBG("Enabling FIFO Interrupts: %d %d", (int)enable_int, (int)use_fifo);
|
||||
uint8_t map_value;
|
||||
uint8_t en_value;
|
||||
if (!enable_int) {
|
||||
map_value = 0;
|
||||
en_value = 0;
|
||||
} else if (!use_fifo) {
|
||||
map_value = BMA255_INT_MAP_1_INT2_DATA;
|
||||
en_value = BMA255_INT_EN_1_DATA;
|
||||
} else {
|
||||
map_value = BMA255_INT_MAP_1_INT2_FIFO_WATERMARK;
|
||||
en_value = BMA255_INT_EN_1_FIFO_WATERMARK;
|
||||
}
|
||||
|
||||
bma255_write_register(BMA255Register_INT_MAP_1, map_value);
|
||||
bma255_write_register(BMA255Register_INT_EN_1, en_value);
|
||||
|
||||
prv_update_accel_interrupts(enable_int, AccelOperatingMode_Data);
|
||||
}
|
||||
|
||||
void accel_set_num_samples(uint32_t num_samples) {
|
||||
// Disable interrupts so they won't fire while we change sampling rate
|
||||
prv_configure_fifo_interrupts(false, false);
|
||||
|
||||
// Workaround some bma255 issues:
|
||||
// Need to use Standby Mode to read/write the FIFO_CONFIG registers.
|
||||
prv_set_accel_power_mode(BMA255PowerMode_Normal); // Need to transition to Normal first
|
||||
prv_set_accel_power_mode(BMA255PowerMode_Standby);
|
||||
|
||||
if (num_samples > BMA255_FIFO_MAX_FRAMES) {
|
||||
num_samples = BMA255_FIFO_MAX_FRAMES;
|
||||
}
|
||||
BMA255_DBG("Setting num samples to: %"PRIu32, num_samples);
|
||||
|
||||
// Note that with the bma255, we do not want to use Bypass mode when
|
||||
// collecting a single sample as this will result in uneven sampling.
|
||||
// The accelerometer will wake up, provide several samples in quick
|
||||
// succession, and then go back to sleep for a period. Looking at the INT2
|
||||
// line shows similar to this:
|
||||
// _ _ _ _ _ _
|
||||
// .... ____| |_| |_| |______________________| |_| |_| |_________ .....
|
||||
//
|
||||
// By using a FIFO of depth 1, the bma255 respects EST mode and will provide
|
||||
// samples at a predictable interval and rate.
|
||||
const bool use_fifo = (num_samples > 0);
|
||||
|
||||
if (use_fifo) {
|
||||
PBL_LOG(LOG_LEVEL_DEBUG, "Enabling FIFO");
|
||||
// Watermark is the number of samples to be collected
|
||||
prv_program_fifo_register(BMA255Register_FIFO_CONFIG_0, num_samples);
|
||||
prv_set_fifo_mode(BMA255FifoMode_Fifo);
|
||||
} else {
|
||||
PBL_LOG(LOG_LEVEL_DEBUG, "Disabling FIFO");
|
||||
prv_set_fifo_mode(BMA255FifoMode_Bypass);
|
||||
}
|
||||
|
||||
prv_set_accel_power_mode(BMA255PowerMode_Normal);
|
||||
prv_set_accel_power_mode(BMA255PowerMode_LowPower1);
|
||||
|
||||
const bool enable_int = (num_samples != 0);
|
||||
prv_configure_fifo_interrupts(enable_int, use_fifo);
|
||||
|
||||
s_fifo_is_enabled = use_fifo;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Shake Detection
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
static void prv_enable_shake_detection(void) {
|
||||
bma255_write_register(BMA255Register_INT_EN_0, BMA255_INT_EN_0_SLOPE_X_EN |
|
||||
BMA255_INT_EN_0_SLOPE_Y_EN |
|
||||
BMA255_INT_EN_0_SLOPE_Z_EN);
|
||||
|
||||
bma255_write_register(BMA255Register_INT_MAP_0, BMA255_INT_MAP_0_INT1_SLOPE);
|
||||
|
||||
// configure the anymotion interrupt to fire after 4 successive
|
||||
// samples are over the threhold specified
|
||||
accel_set_shake_sensitivity_high(false /* sensitivity_high */);
|
||||
bma255_write_register(BMA255Register_INT_5,
|
||||
BMA255_INT_5_SLOPE_DUR_MASK << BMA255_INT_5_SLOPE_DUR_SHIFT);
|
||||
|
||||
prv_enable_operating_mode(AccelOperatingMode_ShakeDetection, BMA255SampleInterval_83HZ);
|
||||
}
|
||||
|
||||
static void prv_disable_shake_detection(void) {
|
||||
// Don't worry about the configuration registers but disable interrupts from the accel
|
||||
bma255_write_register(BMA255Register_INT_EN_0, 0);
|
||||
|
||||
prv_disable_operating_mode(AccelOperatingMode_ShakeDetection);
|
||||
}
|
||||
|
||||
void accel_enable_shake_detection(bool enable) {
|
||||
if (s_shake_detection_enabled == enable) {
|
||||
// the requested change matches what we already have!
|
||||
return;
|
||||
}
|
||||
PBL_LOG(LOG_LEVEL_DEBUG, "%s shake detection", enable ? "Enabling" : "Disabling");
|
||||
|
||||
prv_update_accel_interrupts(enable, AccelOperatingMode_ShakeDetection);
|
||||
if (enable) {
|
||||
prv_enable_shake_detection();
|
||||
} else {
|
||||
prv_disable_shake_detection();
|
||||
}
|
||||
|
||||
s_shake_detection_enabled = enable;
|
||||
}
|
||||
|
||||
void accel_set_shake_sensitivity_high(bool sensitivity_high) {
|
||||
// Configure the threshold level at which the BMI160 will think shake has occurred
|
||||
if (sensitivity_high) {
|
||||
bma255_write_register(BMA255Register_INT_6,
|
||||
BOARD_CONFIG_ACCEL.accel_config.shake_thresholds[AccelThresholdLow]);
|
||||
} else {
|
||||
bma255_write_register(BMA255Register_INT_6,
|
||||
BOARD_CONFIG_ACCEL.accel_config.shake_thresholds[AccelThresholdHigh]);
|
||||
}
|
||||
}
|
||||
|
||||
bool accel_get_shake_detection_enabled(void) {
|
||||
return s_shake_detection_enabled;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Selftest Support
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
static void prv_soft_reset(void) {
|
||||
bma255_write_register(BMA255Register_BGW_SOFTRESET, BMA255_SOFT_RESET_VALUE);
|
||||
psleep(4);
|
||||
}
|
||||
|
||||
// Minimum thresholds for axis delta in mgs at 4G scale
|
||||
static const uint16_t SELFTEST_THRESHOLDS[] = {
|
||||
[BMA255Axis_X] = 800,
|
||||
[BMA255Axis_Y] = 800,
|
||||
[BMA255Axis_Z] = 400,
|
||||
};
|
||||
|
||||
static const char AXIS_NAMES[] = {
|
||||
[BMA255Axis_X] = 'X',
|
||||
[BMA255Axis_Y] = 'Y',
|
||||
[BMA255Axis_Z] = 'Z',
|
||||
};
|
||||
|
||||
static const uint8_t AXIS_REGISTERS[] = {
|
||||
[BMA255Axis_X] = BMA255Register_ACCD_X_LSB,
|
||||
[BMA255Axis_Y] = BMA255Register_ACCD_Y_LSB,
|
||||
[BMA255Axis_Z] = BMA255Register_ACCD_Z_LSB,
|
||||
};
|
||||
|
||||
static int16_t prv_read_axis(BMA255Axis axis, uint8_t *new_data) {
|
||||
uint8_t raw_buf[2];
|
||||
bma255_burst_read(AXIS_REGISTERS[axis], raw_buf, sizeof(raw_buf));
|
||||
int16_t reading = prv_conv_raw_to_12bit(raw_buf);
|
||||
if (new_data) {
|
||||
*new_data = raw_buf[0] & 0x01;
|
||||
}
|
||||
return reading;
|
||||
}
|
||||
|
||||
static bool prv_selftest_axis(BMA255Axis axis) {
|
||||
uint8_t axis_bits;
|
||||
switch (axis) {
|
||||
case BMA255Axis_X:
|
||||
axis_bits = 0x01;
|
||||
break;
|
||||
case BMA255Axis_Y:
|
||||
axis_bits = 0x02;
|
||||
break;
|
||||
case BMA255Axis_Z:
|
||||
axis_bits = 0x03;
|
||||
break;
|
||||
default:
|
||||
WTF;
|
||||
}
|
||||
|
||||
|
||||
// g-range should be 4g for self-test
|
||||
bma255_set_scale(BMA255Scale_4G);
|
||||
|
||||
psleep(2); // wait for a new sample
|
||||
|
||||
uint8_t new_data;
|
||||
int16_t before = prv_read_axis(axis, &new_data);
|
||||
before = prv_raw_to_mgs(before);
|
||||
|
||||
// Positive axis
|
||||
bma255_write_register(BMA255Register_PMU_SELFTEST, axis_bits | SELFTEST_SIGN_POSITIVE);
|
||||
psleep(50);
|
||||
uint8_t new_positive;
|
||||
int16_t positive = prv_read_axis(axis, &new_positive);
|
||||
positive = prv_raw_to_mgs(positive);
|
||||
|
||||
prv_soft_reset();
|
||||
bma255_set_scale(BMA255Scale_4G);
|
||||
|
||||
// Negative axis
|
||||
bma255_write_register(BMA255Register_PMU_SELFTEST, axis_bits | SELFTEST_SIGN_NEGATIVE);
|
||||
psleep(50);
|
||||
uint8_t new_negative;
|
||||
int16_t negative = prv_read_axis(axis, &new_negative);
|
||||
negative = prv_raw_to_mgs(negative);
|
||||
|
||||
prv_soft_reset();
|
||||
|
||||
int delta = positive - negative;
|
||||
delta = abs(delta);
|
||||
|
||||
PBL_LOG(LOG_LEVEL_DEBUG,
|
||||
"Self test axis %c: %d Pos: %d Neg: %d Delta: %d (required %d)",
|
||||
AXIS_NAMES[axis], before, positive,
|
||||
negative, delta, SELFTEST_THRESHOLDS[axis]);
|
||||
|
||||
if (delta < SELFTEST_THRESHOLDS[axis]) {
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Self test failed for axis %c: %d < %d",
|
||||
AXIS_NAMES[axis], delta, SELFTEST_THRESHOLDS[axis]);
|
||||
return false;
|
||||
}
|
||||
|
||||
if ((new_data + new_negative + new_positive) != 3) {
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Self test problem? Not logging data? %d %d %d",
|
||||
new_data, new_positive, new_negative);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool bma255_selftest(void) {
|
||||
// calling selftest_axis resets the device
|
||||
bool pass = true;
|
||||
pass &= prv_selftest_axis(BMA255Axis_X);
|
||||
pass &= prv_selftest_axis(BMA255Axis_Y);
|
||||
pass &= prv_selftest_axis(BMA255Axis_Z);
|
||||
|
||||
// g-range should be 4g to copy the BMI160
|
||||
bma255_set_scale(BMA255Scale_4G);
|
||||
|
||||
return pass;
|
||||
}
|
||||
|
||||
bool accel_run_selftest(void) {
|
||||
return bma255_selftest();
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Debug Commands
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
void command_accel_status(void) {
|
||||
const uint8_t bw = bma255_read_register(BMA255Register_PMU_BW);
|
||||
const uint8_t lpw = bma255_read_register(BMA255Register_PMU_LPW);
|
||||
const uint8_t lp = bma255_read_register(BMA255Register_PMU_LOW_POWER);
|
||||
const uint8_t fifo_cfg0 = bma255_read_register(BMA255Register_FIFO_CONFIG_0);
|
||||
const uint8_t fifo_cfg1 = bma255_read_register(BMA255Register_FIFO_CONFIG_1);
|
||||
const uint8_t fifo_status = bma255_read_register(BMA255Register_FIFO_STATUS);
|
||||
const uint8_t int_map_0 = bma255_read_register(BMA255Register_INT_MAP_0);
|
||||
const uint8_t int_en_0 = bma255_read_register(BMA255Register_INT_EN_0);
|
||||
const uint8_t int_map_1 = bma255_read_register(BMA255Register_INT_MAP_1);
|
||||
const uint8_t int_en_1 = bma255_read_register(BMA255Register_INT_EN_1);
|
||||
const uint8_t int_map_2 = bma255_read_register(BMA255Register_INT_MAP_2);
|
||||
const uint8_t int_en_2 = bma255_read_register(BMA255Register_INT_EN_2);
|
||||
const uint8_t int_status_0 = bma255_read_register(BMA255Register_INT_STATUS_0);
|
||||
const uint8_t int_status_1 = bma255_read_register(BMA255Register_INT_STATUS_1);
|
||||
const uint8_t int_status_2 = bma255_read_register(BMA255Register_INT_STATUS_2);
|
||||
const uint8_t int_status_3 = bma255_read_register(BMA255Register_INT_STATUS_3);
|
||||
|
||||
char buf[64];
|
||||
prompt_send_response_fmt(buf, 64, "(0x10) Bandwidth: 0x%"PRIx8, bw);
|
||||
|
||||
prompt_send_response_fmt(buf, 64, "(0x11) LPW: 0x%"PRIx8, lpw);
|
||||
prompt_send_response_fmt(buf, 64, " suspend: 0x%"PRIx8, (lpw & (1 << 7)) != 0);
|
||||
prompt_send_response_fmt(buf, 64, " lowpower_en: 0x%"PRIx8, (lpw & (1 << 6)) != 0);
|
||||
prompt_send_response_fmt(buf, 64, " deep_suspend: 0x%"PRIx8, (lpw & (1 << 5)) != 0);
|
||||
prompt_send_response_fmt(buf, 64, " sleep_dur: 0x%"PRIx8, (lpw & 0b11110) >> 1);
|
||||
|
||||
prompt_send_response_fmt(buf, 64, "(0x12) Low_Power: 0x%"PRIx8, lp);
|
||||
prompt_send_response_fmt(buf, 64, " lowpower_mode: 0x%"PRIx8, (lp & (1 << 6)) != 0);
|
||||
prompt_send_response_fmt(buf, 64, " sleeptimer_mode: 0x%"PRIx8, (lp & (1 << 5)) != 0);
|
||||
|
||||
prompt_send_response_fmt(buf, 64, "(0x30) FIFO Config 0: 0x%"PRIx8, fifo_cfg0);
|
||||
prompt_send_response_fmt(buf, 64, " Watermark: 0x%"PRIx8, fifo_cfg0 & 0b111111);
|
||||
|
||||
prompt_send_response_fmt(buf, 64, "(0x3e) FIFO Config 1: 0x%"PRIx8, fifo_cfg1);
|
||||
prompt_send_response_fmt(buf, 64, " Mode: 0x%"PRIx8, (fifo_cfg1 & (0x3 << 6)) >> 6);
|
||||
prompt_send_response_fmt(buf, 64, " Data Select: 0x%"PRIx8, fifo_cfg1 & 0x3);
|
||||
|
||||
prompt_send_response_fmt(buf, 64, "(0x0e) Fifo Status: 0x%"PRIx8, fifo_status);
|
||||
prompt_send_response_fmt(buf, 64, " Num Samples: 0x%"PRIx8, (fifo_status & 0x3f));
|
||||
|
||||
prompt_send_response_fmt(buf, 64, "(0x19) Int Map 0: 0x%"PRIx8, int_map_0);
|
||||
prompt_send_response_fmt(buf, 64, "(0x16) Int EN 0: 0x%"PRIx8, int_en_0);
|
||||
|
||||
prompt_send_response_fmt(buf, 64, "(0x1a) Int Map 1: 0x%"PRIx8, int_map_1);
|
||||
prompt_send_response_fmt(buf, 64, "(0x17) Int EN 1: 0x%"PRIx8, int_en_1);
|
||||
|
||||
prompt_send_response_fmt(buf, 64, "(0x1b) Int Map 2: 0x%"PRIx8, int_map_2);
|
||||
prompt_send_response_fmt(buf, 64, "(0x18) Int EN 2: 0x%"PRIx8, int_en_2);
|
||||
|
||||
prompt_send_response_fmt(buf, 64, "(0x0a) Int Status 0: 0x%"PRIx8, int_status_0);
|
||||
prompt_send_response_fmt(buf, 64, "(0x0a) Int Status 1: 0x%"PRIx8, int_status_1);
|
||||
prompt_send_response_fmt(buf, 64, "(0x0b) Int Status 2: 0x%"PRIx8, int_status_2);
|
||||
prompt_send_response_fmt(buf, 64, "(0x0c) Int Status 3: 0x%"PRIx8, int_status_3);
|
||||
}
|
||||
|
||||
void command_accel_selftest(void) {
|
||||
const bool success = accel_run_selftest();
|
||||
char *response = (success) ? "Pass" : "Fail";
|
||||
prompt_send_response(response);
|
||||
}
|
||||
|
||||
void command_accel_softreset(void) {
|
||||
prv_soft_reset();
|
||||
}
|
129
src/fw/drivers/imu/bma255/bma255.h
Normal file
129
src/fw/drivers/imu/bma255/bma255.h
Normal file
|
@ -0,0 +1,129 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
void bma255_init(void);
|
||||
bool bma255_query_whoami(void);
|
||||
|
||||
//! Power Modes
|
||||
//! These are the supported power modes, and some rough estimates on power consumption.
|
||||
//! There is a small set of transitions between power modes that are supported. To make life
|
||||
//! easy, we will always go through Normal Mode, which allows transition to/from all power modes.
|
||||
//! Use this enum to index into the \ref s_power_mode table, which contains configurations for each.
|
||||
typedef enum {
|
||||
BMA255PowerMode_Normal = 0, // 130uA
|
||||
BMA255PowerMode_Suspend, // 2.1uA
|
||||
BMA255PowerMode_Standby, // 62uA
|
||||
BMA255PowerMode_DeepSuspend, // 1uA
|
||||
BMA255PowerMode_LowPower1,
|
||||
BMA255PowerMode_LowPower2,
|
||||
|
||||
BMA255PowerModeCount
|
||||
} BMA255PowerMode;
|
||||
|
||||
//! Tsleep values.
|
||||
//! These are defined to the value we pur into the PMU_LPW register.
|
||||
//! See Table 3 of datasheet: "Sleep Phase Duration"
|
||||
typedef enum {
|
||||
BMA255SleepDuration_0p5ms = 5,
|
||||
BMA255SleepDuration_1ms = 6,
|
||||
BMA255SleepDuration_2ms = 7,
|
||||
BMA255SleepDuration_4ms = 8,
|
||||
BMA255SleepDuration_6ms = 9,
|
||||
BMA255SleepDuration_10ms = 10,
|
||||
BMA255SleepDuration_25ms = 11,
|
||||
BMA255SleepDuration_50ms = 12,
|
||||
BMA255SleepDuration_100ms = 13,
|
||||
BMA255SleepDuration_500ms = 14,
|
||||
BMA255SleepDuration_1000ms = 15,
|
||||
|
||||
BMA255SleepDurationCount
|
||||
} BMA255SleepDuration;
|
||||
|
||||
//! These are the natively supported filter bandwidths of the bma255.
|
||||
//! Note that power consumption is tightly tied to the filter bandwidth. In
|
||||
//! order to run acceptably, we need to keep a bandwidth up in the 500Hz ~ 1kHz range.
|
||||
//! Please see discussion below for more information on Bandwith, TSleep and ODR.
|
||||
typedef enum {
|
||||
BMA255Bandwidth_7p81HZ = 8,
|
||||
BMA255Bandwidth_15p63HZ = 9,
|
||||
BMA255Bandwidth_31p25HZ = 10,
|
||||
BMA255Bandwidth_62p5HZ = 11,
|
||||
BMA255Bandwidth_125HZ = 12,
|
||||
BMA255Bandwidth_250HZ = 13,
|
||||
BMA255Bandwidth_500HZ = 14,
|
||||
BMA255Bandwidth_1000HZ = 15,
|
||||
|
||||
BMA255BandwidthCount
|
||||
} BMA255Bandwidth;
|
||||
|
||||
//! In order to acheive low power consumptions, the BMA255 Output Data Rate (ODR)
|
||||
//! is determined by a combination of:
|
||||
//! - high-bandwidth operating rate:
|
||||
//! Less filtering is done on the bma255, which has a direct impact on power consumption.
|
||||
//! This gives a lower "update time", which in turn means less "active time" of the device.
|
||||
//! The trade-off here is that accelerometer data is a bit more susceptible to noise.
|
||||
//! - sleep time:
|
||||
//! The longer the sleep duration, the less power the device consums.
|
||||
//! After tsleep ms, a sample is taken, and then the device goes back to sleep.
|
||||
//!
|
||||
//! Power measurements on the board have shown we ideally want to run at a BW of 500Hz or 1000Hz.
|
||||
//! Unfortunately, there is an issue with data jumps when running in low power modes.
|
||||
//! At 4G sensitivity, we need to run at a bandwidth lower than 500Hz in order to minimize
|
||||
//! jitter in readings. This means we probably want to stay at 250Hz.
|
||||
//!
|
||||
//! We are using Equidistant Sampling Mode (EST) to ensure that samples are taken
|
||||
//! at equal time distances. See Figure 4 in the datasheet for an explanation of this.
|
||||
//! In EST, a sample is taken every tsample ms, where tsample = (tsleep + wkup_time) [1]
|
||||
//!
|
||||
//! We can _approximate_ actual ODR as the following: [2]
|
||||
//! ODR = (1000 / (tsleep + wkup_time))
|
||||
//! where tsleep holds the property that:
|
||||
//! N = (2 * bw) * (tsleep / 1000) such that N is an Integer. [3][4]
|
||||
//! and wkup_time is taken for the corresponding bandwidth from Table 4 of the datasheet.
|
||||
//!
|
||||
//! [1] This is the best we can gather as a good approximation after a meeting with Bosch.
|
||||
//! [2] This is only an approximation as the BMA255 part is only guaranteed to have
|
||||
//! Bandwidth accuracy within +/- 10%
|
||||
//! [3] See p.16 of datasheet. Note that the formula in the datasheet is confirmed wrong by Bosch.
|
||||
//! [4] Take note that all tsleep values are supported when running at 500Hz
|
||||
//!
|
||||
typedef enum {
|
||||
BMA255ODR_1HZ = 0,
|
||||
BMA255ODR_10HZ,
|
||||
BMA255ODR_19HZ,
|
||||
BMA255ODR_83HZ,
|
||||
BMA255ODR_125HZ,
|
||||
BMA255ODR_166HZ,
|
||||
BMA255ODR_250HZ,
|
||||
|
||||
BMA255ODRCount
|
||||
} BMA255ODR;
|
||||
|
||||
//! Note that these sample intervals are approximations.
|
||||
typedef enum {
|
||||
BMA255SampleInterval_1HZ = (1000000 / 1),
|
||||
BMA255SampleInterval_10HZ = (1000000 / 10),
|
||||
BMA255SampleInterval_19HZ = (1000000 / 19),
|
||||
BMA255SampleInterval_83HZ = (1000000 / 83),
|
||||
BMA255SampleInterval_125HZ = (1000000 / 125),
|
||||
BMA255SampleInterval_166HZ = (1000000 / 166),
|
||||
BMA255SampleInterval_250HZ = (1000000 / 250),
|
||||
} BMA255SampleInterval;
|
49
src/fw/drivers/imu/bma255/bma255_private.h
Normal file
49
src/fw/drivers/imu/bma255/bma255_private.h
Normal file
|
@ -0,0 +1,49 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include "board/board.h"
|
||||
#include "drivers/gpio.h"
|
||||
#include "drivers/periph_config.h"
|
||||
#include "drivers/spi.h"
|
||||
|
||||
#define STM32F2_COMPATIBLE
|
||||
#define STM32F4_COMPATIBLE
|
||||
#include <mcu.h>
|
||||
|
||||
bool bma255_selftest(void);
|
||||
|
||||
void bma255_gpio_init(void);
|
||||
|
||||
void bma255_enable_spi_clock(void);
|
||||
|
||||
void bma255_disable_spi_clock(void);
|
||||
|
||||
uint8_t bma255_send_and_receive_byte(uint8_t byte);
|
||||
|
||||
void bma255_send_byte(uint8_t byte);
|
||||
|
||||
void bma255_prepare_txn(uint8_t address);
|
||||
|
||||
void bma255_end_txn(void);
|
||||
|
||||
void bma255_burst_read(uint8_t address, uint8_t *data, size_t len);
|
||||
|
||||
uint8_t bma255_read_register(uint8_t address);
|
||||
|
||||
void bma255_write_register(uint8_t address, uint8_t data);
|
||||
|
||||
void bma255_read_modify_write(uint8_t reg, uint8_t value, uint8_t mask);
|
188
src/fw/drivers/imu/bma255/bma255_regs.h
Normal file
188
src/fw/drivers/imu/bma255/bma255_regs.h
Normal file
|
@ -0,0 +1,188 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "bma255.h"
|
||||
|
||||
#include "util/attributes.h"
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
// Read & Write flags to be masked onto register addresses for raw spi transactions
|
||||
static const uint8_t BMA255_READ_FLAG = 0x80;
|
||||
static const uint8_t BMA255_WRITE_FLAG = 0x00;
|
||||
|
||||
// BMI255 Register Map
|
||||
typedef enum {
|
||||
BMA255Register_BGW_CHIP_ID = 0x00,
|
||||
BMA255Register_ACCD_X_LSB = 0x02,
|
||||
BMA255Register_ACCD_X_MSB = 0x03,
|
||||
BMA255Register_ACCD_Y_LSB = 0x04,
|
||||
BMA255Register_ACCD_Y_MSB = 0x05,
|
||||
BMA255Register_ACCD_Z_LSB = 0x06,
|
||||
BMA255Register_ACCD_Z_MSB = 0x07,
|
||||
BMA255Register_ACCD_TEMP = 0x08,
|
||||
BMA255Register_INT_STATUS_0 = 0x09,
|
||||
BMA255Register_INT_STATUS_1 = 0x0a,
|
||||
BMA255Register_INT_STATUS_2 = 0x0b,
|
||||
BMA255Register_INT_STATUS_3 = 0x0c,
|
||||
BMA255Register_FIFO_STATUS = 0x0e,
|
||||
BMA255Register_PMU_RANGE = 0x0f,
|
||||
BMA255Register_PMU_BW = 0x10,
|
||||
BMA255Register_PMU_LPW = 0x11,
|
||||
BMA255Register_PMU_LOW_POWER = 0x12,
|
||||
BMA255Register_ACCD_HBW = 0x13,
|
||||
BMA255Register_BGW_SOFTRESET = 0x14,
|
||||
BMA255Register_INT_EN_0 = 0x16,
|
||||
BMA255Register_INT_EN_1 = 0x17,
|
||||
BMA255Register_INT_EN_2 = 0x18,
|
||||
BMA255Register_INT_MAP_0 = 0x19,
|
||||
BMA255Register_INT_MAP_1 = 0x1a,
|
||||
BMA255Register_INT_MAP_2 = 0x1b,
|
||||
BMA255Register_INT_SRC = 0x1e,
|
||||
BMA255Register_INT_OUT_CTRL = 0x20,
|
||||
BMA255Register_INT_RST_LATCH = 0x21,
|
||||
BMA255Register_INT_0 = 0x22,
|
||||
BMA255Register_INT_1 = 0x23,
|
||||
BMA255Register_INT_2 = 0x24,
|
||||
BMA255Register_INT_3 = 0x25,
|
||||
BMA255Register_INT_4 = 0x26,
|
||||
BMA255Register_INT_5 = 0x27,
|
||||
BMA255Register_INT_6 = 0x28,
|
||||
BMA255Register_INT_7 = 0x29,
|
||||
BMA255Register_INT_8 = 0x2a,
|
||||
BMA255Register_INT_9 = 0x2b,
|
||||
BMA255Register_INT_a = 0x2c,
|
||||
BMA255Register_INT_b = 0x2d,
|
||||
BMA255Register_INT_c = 0x2e,
|
||||
BMA255Register_INT_d = 0x2f,
|
||||
BMA255Register_FIFO_CONFIG_0 = 0x30,
|
||||
BMA255Register_PMU_SELFTEST = 0x32,
|
||||
BMA255Register_TRIM_NVM_CTRL = 0x33,
|
||||
BMA255Register_BGW_SPI3_WDT = 0x34,
|
||||
BMA255Register_OFC_CTRL = 0x36,
|
||||
BMA255Register_OFC_SETTINGS = 0x37,
|
||||
BMA255Register_OFC_OFFSET_X = 0x38,
|
||||
BMA255Register_OFC_OFFSET_Y = 0x39,
|
||||
BMA255Register_OFC_OFFSET_Z = 0x3a,
|
||||
BMA255Register_TRIM_GPO0 = 0x3b,
|
||||
BMA255Register_TRIM_GP1 = 0x3c,
|
||||
BMA255Register_FIFO_CONFIG_1 = 0x3e,
|
||||
BMA255Register_FIFO_DATA = 0x3f,
|
||||
|
||||
BMA255Register_EXTENDED_MEMORY_MAP = 0x35,
|
||||
BMA255Register_TEMPERATURE_SENSOR_CTRL = 0x4f,
|
||||
} BMA255Register;
|
||||
|
||||
static const uint8_t BMA255_EXTENDED_MEMORY_MAP_OPEN = 0xca;
|
||||
static const uint8_t BMA255_EXTENDED_MEMORY_MAP_CLOSE = 0x0a;
|
||||
|
||||
static const uint8_t BMA255_TEMPERATURE_SENSOR_DISABLE = 0x0;
|
||||
|
||||
static const uint8_t BMA255_CHIP_ID = 0xfa;
|
||||
static const uint8_t BMA255_ACC_CONF_PMU_BW_MASK = 0x1f;
|
||||
static const uint8_t BMA255_SOFT_RESET_VALUE = 0xb6;
|
||||
|
||||
static const uint8_t BMA255_FIFO_MODE_SHIFT = 6;
|
||||
static const uint8_t BMA255_FIFO_MODE_MASK = 0xc0;
|
||||
|
||||
static const uint8_t BMA255_FIFO_DATA_SEL_SHIFT = 0;
|
||||
static const uint8_t BMA255_FIFO_DATA_SEL_MASK = 0x03;
|
||||
|
||||
static const uint8_t BMA255_INT_STATUS_0_SLOPE_MASK = (1 << 2);
|
||||
|
||||
static const uint8_t BMA255_INT_STATUS_2_SLOPE_SIGN = (1 << 3);
|
||||
static const uint8_t BMA255_INT_STATUS_2_SLOPE_FIRST_X = (1 << 0);
|
||||
static const uint8_t BMA255_INT_STATUS_2_SLOPE_FIRST_Y = (1 << 1);
|
||||
static const uint8_t BMA255_INT_STATUS_2_SLOPE_FIRST_Z = (1 << 2);
|
||||
|
||||
static const uint8_t BMA255_INT_MAP_1_INT2_FIFO_FULL = (0x1 << 5);
|
||||
static const uint8_t BMA255_INT_MAP_1_INT2_FIFO_WATERMARK = (0x1 << 6);
|
||||
static const uint8_t BMA255_INT_MAP_1_INT2_DATA = (0x1 << 7);
|
||||
|
||||
static const uint8_t BMA255_INT_MAP_0_INT1_SLOPE = (0x1 << 2);
|
||||
|
||||
static const uint8_t BMA255_INT_EN_0_SLOPE_X_EN = (1 << 0);
|
||||
static const uint8_t BMA255_INT_EN_0_SLOPE_Y_EN = (1 << 1);
|
||||
static const uint8_t BMA255_INT_EN_0_SLOPE_Z_EN = (1 << 2);
|
||||
|
||||
static const uint8_t BMA255_INT_EN_1_DATA = (0x1 << 4);
|
||||
static const uint8_t BMA255_INT_EN_1_FIFO_FULL = (0x1 << 5);
|
||||
static const uint8_t BMA255_INT_EN_1_FIFO_WATERMARK = (0x1 << 6);
|
||||
|
||||
static const uint8_t BMA255_INT_5_SLOPE_DUR_SHIFT = 0;
|
||||
static const uint8_t BMA255_INT_5_SLOPE_DUR_MASK = 0x3;
|
||||
|
||||
static const uint8_t BMA255_LPW_SLEEP_DUR_SHIFT = 1;
|
||||
static const uint8_t BMA255_LPW_SLEEP_DUR_MASK = (0xf << 1);
|
||||
|
||||
static const uint8_t BMA255_LPW_POWER_SHIFT = 5;
|
||||
static const uint8_t BMA255_LPW_POWER_MASK = (0x7 << 5);
|
||||
|
||||
static const uint8_t BMA255_LOW_POWER_SHIFT = 5;
|
||||
static const uint8_t BMA255_LOW_POWER_MASK = (0x3 << 5);
|
||||
|
||||
typedef struct PACKED {
|
||||
uint16_t x;
|
||||
uint16_t y;
|
||||
uint16_t z;
|
||||
} BMA255AccelData;
|
||||
|
||||
typedef enum {
|
||||
BMA255FifoMode_Bypass = 0x00,
|
||||
BMA255FifoMode_Fifo = 0x01,
|
||||
BMA255FifoMode_Stream = 0x02,
|
||||
} BMA255FifoMode;
|
||||
|
||||
typedef enum {
|
||||
BMA255FifoDataSel_XYZ = 0x00,
|
||||
BMA255FifoDataSel_X = 0x01,
|
||||
BMA255FifoDataSel_Y = 0x02,
|
||||
BMA255FifoDataSel_Z = 0x03,
|
||||
} BMA255FifoDataSel;
|
||||
|
||||
//! Configuration to be used to enter each of the supported power modes.
|
||||
//! Make sure that the PMU_LOW_POWER register is always set prior to the PMU_LPW
|
||||
//! register, as the bma255 uses this restriction.
|
||||
static const struct {
|
||||
uint8_t low_power; //!< PMU_LOW_POWER register
|
||||
uint8_t lpw; //!< PMU_LPW register
|
||||
} s_power_mode[BMA255PowerModeCount] = {
|
||||
[BMA255PowerMode_Normal] = { .low_power = 0x0, .lpw = 0x0 },
|
||||
[BMA255PowerMode_Suspend] = { .low_power = 0x0, .lpw = 0x4 },
|
||||
[BMA255PowerMode_Standby] = { .low_power = 0x2, .lpw = 0x4 },
|
||||
[BMA255PowerMode_DeepSuspend] = { .low_power = 0x0, .lpw = 0x1 },
|
||||
[BMA255PowerMode_LowPower1] = { .low_power = 0x1, .lpw = 0x2 },
|
||||
[BMA255PowerMode_LowPower2] = { .low_power = 0x3, .lpw = 0x2 },
|
||||
};
|
||||
|
||||
//! Configuration to be used for each ODR we will be using.
|
||||
//! This involves some native bma255 bandwidth and a tsleep value.
|
||||
//! Errata states that at 4G sensitivity, we need to run at a bandwidth of 250HZ or lower.
|
||||
//! See the discussion around \ref BMA255ODR for more information.
|
||||
static const struct {
|
||||
BMA255Bandwidth bw;
|
||||
BMA255SleepDuration tsleep;
|
||||
} s_odr_settings[BMA255ODRCount] = {
|
||||
[BMA255ODR_1HZ] = { BMA255Bandwidth_250HZ, BMA255SleepDuration_1000ms },
|
||||
[BMA255ODR_10HZ] = { BMA255Bandwidth_250HZ, BMA255SleepDuration_100ms },
|
||||
[BMA255ODR_19HZ] = { BMA255Bandwidth_250HZ, BMA255SleepDuration_50ms },
|
||||
[BMA255ODR_83HZ] = { BMA255Bandwidth_250HZ, BMA255SleepDuration_10ms },
|
||||
[BMA255ODR_125HZ] = { BMA255Bandwidth_250HZ, BMA255SleepDuration_6ms },
|
||||
[BMA255ODR_166HZ] = { BMA255Bandwidth_250HZ, BMA255SleepDuration_4ms },
|
||||
[BMA255ODR_250HZ] = { BMA255Bandwidth_250HZ, BMA255SleepDuration_2ms },
|
||||
};
|
158
src/fw/drivers/imu/bma255/bma255_spi.c
Normal file
158
src/fw/drivers/imu/bma255/bma255_spi.c
Normal file
|
@ -0,0 +1,158 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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 "board/board.h"
|
||||
#include "drivers/gpio.h"
|
||||
#include "drivers/periph_config.h"
|
||||
#include "drivers/rtc.h"
|
||||
#include "drivers/spi.h"
|
||||
#include "kernel/util/sleep.h"
|
||||
#include "util/units.h"
|
||||
|
||||
|
||||
#define STM32F2_COMPATIBLE
|
||||
#define STM32F4_COMPATIBLE
|
||||
#include <mcu.h>
|
||||
|
||||
#include "bma255_private.h"
|
||||
#include "bma255_regs.h"
|
||||
|
||||
#define BMA255_SPI SPI3
|
||||
static const uint32_t BMA255_PERIPH_CLOCK = RCC_APB1Periph_SPI3;
|
||||
static const SpiPeriphClock BMA255_SPI_CLOCK = SpiPeriphClockAPB1;
|
||||
|
||||
static const AfConfig BMA255_SCLK_CONFIG = { GPIOB, GPIO_Pin_12, GPIO_PinSource12, GPIO_AF7_SPI3 };
|
||||
static const AfConfig BMA255_MISO_CONFIG = { GPIOC, GPIO_Pin_11, GPIO_PinSource11, GPIO_AF_SPI3 };
|
||||
static const AfConfig BMA255_MOSI_CONFIG = { GPIOC, GPIO_Pin_12, GPIO_PinSource12, GPIO_AF_SPI3 };
|
||||
static const OutputConfig BMA255_SCS_CONFIG = { GPIOA, GPIO_Pin_4, false };
|
||||
|
||||
// Need to wait a minimum of 450µs after a write.
|
||||
// Due to RTC resolution, we need to make sure that the tick counter has
|
||||
// incremented twice so we can be certain at least one full tick-period has elapsed.
|
||||
#define MIN_TICKS_AFTER_WRITE 2
|
||||
static RtcTicks s_last_write_ticks = 0;
|
||||
_Static_assert(RTC_TICKS_HZ < (1000000 / 450), "Tick period < 450µs");
|
||||
|
||||
void bma255_gpio_init(void) {
|
||||
periph_config_acquire_lock();
|
||||
|
||||
gpio_af_init(&BMA255_SCLK_CONFIG, GPIO_OType_PP, GPIO_Speed_50MHz, GPIO_PuPd_NOPULL);
|
||||
gpio_af_init(&BMA255_MISO_CONFIG, GPIO_OType_PP, GPIO_Speed_50MHz, GPIO_PuPd_NOPULL);
|
||||
gpio_af_init(&BMA255_MOSI_CONFIG, GPIO_OType_PP, GPIO_Speed_50MHz, GPIO_PuPd_NOPULL);
|
||||
gpio_output_init(&BMA255_SCS_CONFIG, GPIO_OType_PP, GPIO_Speed_50MHz);
|
||||
|
||||
SPI_InitTypeDef spi_cfg;
|
||||
SPI_I2S_DeInit(BMA255_SPI);
|
||||
spi_cfg.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
|
||||
spi_cfg.SPI_Mode = SPI_Mode_Master;
|
||||
spi_cfg.SPI_DataSize = SPI_DataSize_8b;
|
||||
spi_cfg.SPI_CPOL = SPI_CPOL_Low;
|
||||
spi_cfg.SPI_CPHA = SPI_CPHA_1Edge;
|
||||
spi_cfg.SPI_NSS = SPI_NSS_Soft;
|
||||
// Max SCLK frequency for the BMA255 is 10 MHz
|
||||
spi_cfg.SPI_BaudRatePrescaler = spi_find_prescaler(MHZ_TO_HZ(5), BMA255_SPI_CLOCK);
|
||||
spi_cfg.SPI_FirstBit = SPI_FirstBit_MSB;
|
||||
spi_cfg.SPI_CRCPolynomial = 7;
|
||||
|
||||
bma255_enable_spi_clock();
|
||||
SPI_Init(BMA255_SPI, &spi_cfg);
|
||||
SPI_Cmd(BMA255_SPI, ENABLE);
|
||||
bma255_disable_spi_clock();
|
||||
|
||||
periph_config_release_lock();
|
||||
}
|
||||
|
||||
void bma255_enable_spi_clock(void) {
|
||||
periph_config_enable(BMA255_SPI, BMA255_PERIPH_CLOCK);
|
||||
}
|
||||
|
||||
void bma255_disable_spi_clock(void) {
|
||||
periph_config_disable(BMA255_SPI, BMA255_PERIPH_CLOCK);
|
||||
}
|
||||
|
||||
uint8_t bma255_send_and_receive_byte(uint8_t byte) {
|
||||
// Ensure that there are no other write operations in progress
|
||||
while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_TXE) == RESET) {}
|
||||
// Send the byte on the SPI bus
|
||||
SPI_I2S_SendData(BMA255_SPI, byte);
|
||||
|
||||
// Wait for the response byte to be received
|
||||
while (SPI_I2S_GetFlagStatus(BMA255_SPI, SPI_I2S_FLAG_RXNE) == RESET) {}
|
||||
// Return the byte
|
||||
return SPI_I2S_ReceiveData(BMA255_SPI);
|
||||
}
|
||||
|
||||
void bma255_send_byte(uint8_t byte) {
|
||||
// Ensure that there are no other write operations in progress
|
||||
while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_TXE) == RESET) {}
|
||||
// Send the byte on the SPI bus
|
||||
SPI_I2S_SendData(BMA255_SPI, byte);
|
||||
}
|
||||
|
||||
//! Set SCS for transaction, start spi clock, and send address
|
||||
void bma255_prepare_txn(uint8_t address) {
|
||||
while (rtc_get_ticks() < (s_last_write_ticks + MIN_TICKS_AFTER_WRITE)) {
|
||||
psleep(1);
|
||||
}
|
||||
|
||||
gpio_output_set(&BMA255_SCS_CONFIG, true);
|
||||
bma255_enable_spi_clock();
|
||||
bma255_send_and_receive_byte(address);
|
||||
}
|
||||
|
||||
//! Disables spi clock and sets SCS to end txn
|
||||
void bma255_end_txn(void) {
|
||||
bma255_disable_spi_clock();
|
||||
gpio_output_set(&BMA255_SCS_CONFIG, false);
|
||||
}
|
||||
|
||||
void bma255_burst_read(uint8_t address, uint8_t *data, size_t len) {
|
||||
const uint8_t reg = address | BMA255_READ_FLAG;
|
||||
|
||||
bma255_prepare_txn(reg);
|
||||
for (size_t i = 0; i < len; ++i) {
|
||||
data[i] = bma255_send_and_receive_byte(0);
|
||||
}
|
||||
bma255_end_txn();
|
||||
}
|
||||
|
||||
uint8_t bma255_read_register(uint8_t address) {
|
||||
const uint8_t reg = address | BMA255_READ_FLAG;
|
||||
|
||||
bma255_prepare_txn(reg);
|
||||
// Read data
|
||||
uint8_t data = bma255_send_and_receive_byte(0);
|
||||
bma255_end_txn();
|
||||
|
||||
return data;
|
||||
}
|
||||
|
||||
void bma255_write_register(uint8_t address, uint8_t data) {
|
||||
const uint8_t reg = address | BMA255_WRITE_FLAG;
|
||||
|
||||
bma255_prepare_txn(reg);
|
||||
bma255_send_and_receive_byte(data);
|
||||
bma255_end_txn();
|
||||
|
||||
s_last_write_ticks = rtc_get_ticks();
|
||||
}
|
||||
|
||||
void bma255_read_modify_write(uint8_t reg, uint8_t value, uint8_t mask) {
|
||||
uint8_t new_val = bma255_read_register(reg);
|
||||
new_val &= ~mask;
|
||||
new_val |= value;
|
||||
bma255_write_register(reg, new_val);
|
||||
}
|
1078
src/fw/drivers/imu/bmi160/bmi160.c
Normal file
1078
src/fw/drivers/imu/bmi160/bmi160.c
Normal file
File diff suppressed because it is too large
Load diff
36
src/fw/drivers/imu/bmi160/bmi160.h
Normal file
36
src/fw/drivers/imu/bmi160/bmi160.h
Normal file
|
@ -0,0 +1,36 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
typedef enum {
|
||||
BMI160_Accel_Mode_Suspend = 0b00,
|
||||
BMI160_Accel_Mode_Normal = 0b01,
|
||||
BMI160_Accel_Mode_Low = 0b10,
|
||||
} BMI160AccelPowerMode;
|
||||
|
||||
typedef enum {
|
||||
BMI160_Gyro_Mode_Suspend = 0b00,
|
||||
BMI160_Gyro_Mode_Normal = 0b01,
|
||||
BMI160_Gyro_Mode_FastStartup = 0b11
|
||||
} BMI160GyroPowerMode;
|
||||
|
||||
void bmi160_init(void);
|
||||
bool bmi160_query_whoami(void);
|
||||
void bmi160_set_accel_power_mode(BMI160AccelPowerMode mode);
|
||||
void bmi160_set_gyro_power_mode(BMI160GyroPowerMode mode);
|
30
src/fw/drivers/imu/bmi160/bmi160_private.h
Normal file
30
src/fw/drivers/imu/bmi160/bmi160_private.h
Normal file
|
@ -0,0 +1,30 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include "board/board.h"
|
||||
#include "drivers/gpio.h"
|
||||
#include "drivers/periph_config.h"
|
||||
#include "drivers/spi.h"
|
||||
|
||||
//! Ask the chip to accept input from the SPI bus. Required after issuing a soft reset.
|
||||
void bmi160_enable_spi_mode(void);
|
||||
void bmi160_begin_burst(uint8_t addr);
|
||||
void bmi160_end_burst(void);
|
||||
|
||||
uint8_t bmi160_read_reg(uint8_t reg);
|
||||
uint16_t bmi160_read_16bit_reg(uint8_t reg);
|
||||
void bmi160_write_reg(uint8_t reg, uint8_t value);
|
333
src/fw/drivers/imu/bmi160/bmi160_regs.h
Normal file
333
src/fw/drivers/imu/bmi160/bmi160_regs.h
Normal file
|
@ -0,0 +1,333 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
// BMI160 Register Map
|
||||
static const uint8_t BMI160_REG_CHIP_ID = 0x00;
|
||||
static const uint8_t BMI160_REG_ERR = 0x02;
|
||||
static const uint8_t BMI160_REG_PMU_STATUS = 0x03;
|
||||
|
||||
// Current sensor data
|
||||
static const uint8_t BMI160_REG_DATA_0 = 0x04;
|
||||
// Register names differ from those in datasheet
|
||||
static const uint8_t BMI160_REG_MAG_X_LSB = 0x04;
|
||||
static const uint8_t BMI160_REG_MAG_X_MSB = 0x05;
|
||||
static const uint8_t BMI160_REG_MAG_Y_LSB = 0x06;
|
||||
static const uint8_t BMI160_REG_MAG_Y_MSB = 0x07;
|
||||
static const uint8_t BMI160_REG_MAG_Z_LSB = 0x08;
|
||||
static const uint8_t BMI160_REG_MAG_Z_MSB = 0x09;
|
||||
static const uint8_t BMI160_REG_RHALL_LSB = 0x0A;
|
||||
static const uint8_t BMI160_REG_RHALL_MSB = 0x0B;
|
||||
static const uint8_t BMI160_REG_GYR_X_LSB = 0x0C;
|
||||
static const uint8_t BMI160_REG_GYR_X_MSB = 0x0D;
|
||||
static const uint8_t BMI160_REG_GYR_Y_LSB = 0x0E;
|
||||
static const uint8_t BMI160_REG_GYR_Y_MSB = 0x0F;
|
||||
static const uint8_t BMI160_REG_GYR_Z_LSB = 0x10;
|
||||
static const uint8_t BMI160_REG_GYR_Z_MSB = 0x11;
|
||||
static const uint8_t BMI160_REG_ACC_X_LSB = 0x12;
|
||||
static const uint8_t BMI160_REG_ACC_X_MSB = 0x13;
|
||||
static const uint8_t BMI160_REG_ACC_Y_LSB = 0x14;
|
||||
static const uint8_t BMI160_REG_ACC_Y_MSB = 0x15;
|
||||
static const uint8_t BMI160_REG_ACC_Z_LSB = 0x16;
|
||||
static const uint8_t BMI160_REG_ACC_Z_MSB = 0x17;
|
||||
// Sensor time is stored in 24-bit little-endian format (LSB in BMI160_REG_SENSORTIME_0)
|
||||
static const uint8_t BMI160_REG_SENSORTIME_0 = 0x18;
|
||||
static const uint8_t BMI160_REG_SENSORTIME_1 = 0x19;
|
||||
static const uint8_t BMI160_REG_SENSORTIME_2 = 0x1A;
|
||||
|
||||
static const uint8_t BMI160_SENSORTIME_RESOLUTION_US = 39;
|
||||
|
||||
// Status registers
|
||||
static const uint8_t BMI160_REG_STATUS = 0x1B;
|
||||
static const uint8_t BMI160_REG_INT_STATUS_0 = 0x1C;
|
||||
static const uint8_t BMI160_REG_INT_STATUS_1 = 0x1D;
|
||||
static const uint8_t BMI160_REG_INT_STATUS_2 = 0x1E;
|
||||
static const uint8_t BMI160_REG_INT_STATUS_3 = 0x1F;
|
||||
static const uint8_t BMI160_REG_TEMPERATURE_LSB = 0x20;
|
||||
static const uint8_t BMI160_REG_TEMPERATURE_MSB = 0x21;
|
||||
|
||||
// FIFO
|
||||
static const uint8_t BMI160_REG_FIFO_LENGTH_LSB = 0x22;
|
||||
static const uint8_t BMI160_REG_FIFO_LENGTH_MSB = 0x23;
|
||||
static const uint8_t BMI160_REG_FIFO_DOWNS = 0x45;
|
||||
static const uint8_t BMI160_REG_FIFO_CONFIG_0 = 0x46;
|
||||
static const uint8_t BMI160_REG_FIFO_CONFIG_1 = 0x47;
|
||||
static const uint8_t BMI160_REG_FIFO_DATA = 0x24;
|
||||
|
||||
static const uint8_t BMI160_REG_ACC_CONF = 0x40;
|
||||
static const uint8_t BMI160_REG_ACC_RANGE = 0x41;
|
||||
static const uint8_t BMI160_REG_GYR_CONF = 0x42;
|
||||
static const uint8_t BMI160_REG_GYR_RANGE = 0x43;
|
||||
static const uint8_t BMI160_REG_MAG_CONF = 0x44;
|
||||
|
||||
// Magnetometer interface configuration
|
||||
static const uint8_t BMI160_REG_MAG_I2C_DEVICE_ADDR = 0x4B;
|
||||
static const uint8_t BMI160_REG_MAG_IF_1 = 0x4C;
|
||||
static const uint8_t BMI160_MAG_IF_1_MANUAL_MODE_ENABLE = 0x80;
|
||||
|
||||
static const uint8_t BMI160_REG_MAG_READ_ADDR = 0x4D;
|
||||
static const uint8_t BMI160_REG_MAG_WRITE_ADDR = 0x4E;
|
||||
static const uint8_t BMI160_REG_MAG_WRITE_DATA = 0x4F;
|
||||
|
||||
// Interrupt configuration
|
||||
static const uint8_t BMI160_REG_INT_EN_0 = 0x50;
|
||||
static const uint8_t BMI160_REG_INT_EN_1 = 0x51;
|
||||
static const uint8_t BMI160_REG_INT_EN_2 = 0x52;
|
||||
static const uint8_t BMI160_REG_INT_OUT_CTRL = 0x53;
|
||||
static const uint8_t BMI160_REG_INT_LATCH = 0x54;
|
||||
static const uint8_t BMI160_REG_INT_MAP_0 = 0x55;
|
||||
static const uint8_t BMI160_REG_INT_MAP_1 = 0x56;
|
||||
static const uint8_t BMI160_REG_INT_MAP_2 = 0x57;
|
||||
static const uint8_t BMI160_REG_INT_DATA_0 = 0x58;
|
||||
static const uint8_t BMI160_REG_INT_DATA_1 = 0x59;
|
||||
static const uint8_t BMI160_REG_INT_LOWHIGH_0 = 0x5A;
|
||||
static const uint8_t BMI160_REG_INT_LOWHIGH_1 = 0x5B;
|
||||
static const uint8_t BMI160_REG_INT_LOWHIGH_2 = 0x5C;
|
||||
static const uint8_t BMI160_REG_INT_LOWHIGH_3 = 0x5D;
|
||||
static const uint8_t BMI160_REG_INT_LOWHIGH_4 = 0x5E;
|
||||
static const uint8_t BMI160_REG_INT_MOTION_0 = 0x5F;
|
||||
static const uint8_t BMI160_REG_INT_MOTION_1 = 0x60;
|
||||
static const uint8_t BMI160_REG_INT_MOTION_2 = 0x61;
|
||||
static const uint8_t BMI160_REG_INT_MOTION_3 = 0x62;
|
||||
static const uint8_t BMI160_REG_INT_TAP_0 = 0x63;
|
||||
static const uint8_t BMI160_REG_INT_TAP_1 = 0x64;
|
||||
static const uint8_t BMI160_REG_INT_ORIENT_0 = 0x65;
|
||||
static const uint8_t BMI160_REG_INT_ORIENT_1 = 0x66;
|
||||
static const uint8_t BMI160_REG_INT_FLAT_0 = 0x67;
|
||||
static const uint8_t BMI160_REG_INT_FLAT_1 = 0x68;
|
||||
|
||||
static const uint8_t BMI160_REG_FOC_CONF = 0x69;
|
||||
static const uint8_t BMI160_REG_CONF = 0x6A;
|
||||
|
||||
static const uint8_t BMI160_REG_IF_CONF = 0x6B;
|
||||
static const uint8_t BMI160_IF_CONF_MAG_ENABLE = 0x20; // Undocumented
|
||||
|
||||
static const uint8_t BMI160_REG_PMU_TRIGGER = 0x6C;
|
||||
static const uint8_t BMI160_REG_SELF_TEST = 0x6D;
|
||||
static const uint8_t BMI160_REG_NV_CONF = 0x70;
|
||||
|
||||
static const uint8_t BMI160_REG_OFFSET_ACC_X = 0x71;
|
||||
static const uint8_t BMI160_REG_OFFSET_ACC_Y = 0x72;
|
||||
static const uint8_t BMI160_REG_OFFSET_ACC_Z = 0x73;
|
||||
static const uint8_t BMI160_REG_OFFSET_GYR_X_LSB = 0x74;
|
||||
static const uint8_t BMI160_REG_OFFSET_GYR_Y_LSB = 0x75;
|
||||
static const uint8_t BMI160_REG_OFFSET_GYR_Z_LSB = 0x76;
|
||||
static const uint8_t BMI160_REG_OFFSET_7 = 0x77;
|
||||
|
||||
static const uint8_t BMI160_REG_STEPCOUNTER_LSB = 0x78;
|
||||
static const uint8_t BMI160_REG_STEPCOUNTER_MSB = 0x79;
|
||||
static const uint8_t BMI160_REG_INT_STEP_DET_CONF_0 = 0x7A;
|
||||
static const uint8_t BMI160_REG_STEPCOUNTER_CONF = 0x7B;
|
||||
|
||||
static const uint8_t BMI160_REG_CMD = 0x7E;
|
||||
static const uint8_t BMI160_CMD_START_FOC = 0x03;
|
||||
// To set the PMU mode, bitwise-or the command with the desired mode
|
||||
// from the BMI160*PowerMode enums.
|
||||
static const uint8_t BMI160_CMD_ACC_SET_PMU_MODE = 0x10;
|
||||
static const uint8_t BMI160_CMD_GYR_SET_PMU_MODE = 0x14;
|
||||
static const uint8_t BMI160_CMD_MAG_SET_PMU_MODE = 0x18;
|
||||
|
||||
static const uint8_t BMI160_CMD_PROG_NVM = 0xA0;
|
||||
static const uint8_t BMI160_CMD_FIFO_FLUSH = 0xB0;
|
||||
static const uint8_t BMI160_CMD_INT_RESET = 0xB1;
|
||||
static const uint8_t BMI160_CMD_SOFTRESET = 0xB6;
|
||||
static const uint8_t BMI160_CMD_STEP_CNT_CLR = 0xB2;
|
||||
|
||||
// Command sequence to enable "extended mode"
|
||||
static const uint8_t BMI160_CMD_EXTMODE_EN_FIRST = 0x37;
|
||||
static const uint8_t BMI160_CMD_EXTMODE_EN_MIDDLE = 0x9A;
|
||||
static const uint8_t BMI160_CMD_EXTMODE_EN_LAST = 0xc0;
|
||||
|
||||
// Extended mode register; see Bosch Android driver
|
||||
static const uint8_t BMI160_REG_EXT_MODE = 0x7F;
|
||||
static const uint8_t BMI160_EXT_MODE_PAGING_EN = 0x80;
|
||||
static const uint8_t BMI160_EXT_MODE_TARGET_PAGE_1 = 0x10;
|
||||
|
||||
// Constants
|
||||
static const uint8_t BMI160_CHIP_ID = 0xD1;
|
||||
|
||||
static const uint8_t BMI160_REG_MASK = 0x7F; // Register address is 7 bits wide
|
||||
static const uint8_t BMI160_READ_FLAG = 0x80;
|
||||
|
||||
/*
|
||||
* Register Definitions
|
||||
*/
|
||||
|
||||
// ACC_CONF
|
||||
static const uint8_t BMI160_ACC_CONF_ACC_US_MASK = 0x1;
|
||||
static const uint8_t BMI160_ACC_CONF_ACC_US_SHIFT = 7;
|
||||
static const uint8_t BMI160_ACC_CONF_ACC_BWP_SHIFT = 4;
|
||||
static const uint8_t BMI160_ACC_CONF_ACC_BWP_MASK = 0x7;
|
||||
static const uint8_t BMI160_ACC_CONF_ACC_ODR_SHIFT = 0;
|
||||
static const uint8_t BMI160_ACC_CONF_ACC_ODR_MASK = 0xf;
|
||||
|
||||
// Hz = 100 / 2 ^ (8 - ACC_ODR_VAL)
|
||||
typedef enum { /* value is the interval in microseconds */
|
||||
BMI160SampleRate_12p5_HZ = 80000,
|
||||
BMI160SampleRate_25_HZ = 40000,
|
||||
BMI160SampleRate_50_HZ = 20000,
|
||||
BMI160SampleRate_100_HZ = 10000,
|
||||
BMI160SampleRate_200_HZ = 5000,
|
||||
BMI160SampleRate_400_HZ = 2500,
|
||||
BMI160SampleRate_800_HZ = 1250,
|
||||
BMI160SampleRate_1600_HZ = 625,
|
||||
} BMI160SampleRate;
|
||||
|
||||
typedef enum { /* value matches ACC_CONF ODR setting that must be programmed */
|
||||
BMI160AccODR_12p5_HZ = 5,
|
||||
BMI160AccODR_25_HZ = 6,
|
||||
BMI160AccODR_50_HZ = 7,
|
||||
BMI160AccODR_100_HZ = 8,
|
||||
BMI160AccODR_200_HZ = 9,
|
||||
BMI160AccODR_400_HZ = 10,
|
||||
BMI160AccODR_800_HZ = 11,
|
||||
BMI160AccODR_1600_HZ = 12,
|
||||
} BMI160AccODR;
|
||||
// TODO: Create a better way to change the frequency
|
||||
|
||||
#define BMI160_ACC_CONF_ODR_RESET_VALUE_US BMI160SampleRate_100_HZ
|
||||
|
||||
#define BMI160_ACC_CONF_NORMAL_BODE_US_AND_BWP 0x2
|
||||
#define BMI160_DEFAULT_ACC_CONF_VALUE \
|
||||
((BMI160_ACC_CONF_NORMAL_BODE_US_AND_BWP << BMI160_ACC_CONF_ACC_BWP_SHIFT) | \
|
||||
(BMI160AccODR_50_HZ << BMI160_ACC_CONF_ACC_ODR_SHIFT))
|
||||
|
||||
#define BMI160_ACC_CONF_SELF_TEST_VALUE 0x2c // See 2.8.1 of bmi160 data sheet
|
||||
|
||||
// Values for BMI160_REG_ACC_RANGE
|
||||
static const uint8_t BMI160_ACC_RANGE_2G = 0x3;
|
||||
static const uint8_t BMI160_ACC_RANGE_4G = 0x5;
|
||||
static const uint8_t BMI160_ACC_RANGE_8G = 0x8;
|
||||
static const uint8_t BMI160_ACC_RANGE_16G = 0xC;
|
||||
|
||||
// STATUS
|
||||
static const uint8_t BMI160_STATUS_DRDY_ACC_MASK = (1 << 7);
|
||||
static const uint8_t BMI160_STATUS_DRDY_GYR_MASK = (1 << 6);
|
||||
static const uint8_t BMI160_STATUS_DRDY_MAG_MASK = (1 << 5);
|
||||
static const uint8_t BMI160_STATUS_NVM_RDY_MASK = (1 << 4);
|
||||
static const uint8_t BMI160_STATUS_FOC_RDY_MASK = (1 << 3);
|
||||
static const uint8_t BMI160_STATUS_MAG_MAN_OP_MASK = (1 << 2);
|
||||
static const uint8_t BMI160_STATUS_GYR_SELF_TEST_OK_MASK = (1 << 1);
|
||||
static const uint8_t BMI160_STATUS_GYR_POR_DETECTED_MASK = (1 << 0);
|
||||
|
||||
// INT_TAP[0]
|
||||
static const uint8_t BMI160_INT_TAP_QUIET_SHIFT = 7;
|
||||
static const uint8_t BMI160_INT_TAP_SHOCK_SHIFT = 6;
|
||||
// bits 5:3 reserved
|
||||
static const uint8_t BMI160_INT_TAP_DUR_SHIFT = 0;
|
||||
|
||||
// INT_TAP[1] Register Definition
|
||||
// bit 7 reserved
|
||||
static const uint8_t BMI160_INT_TAP_THRESH_SHIFT = 4;
|
||||
|
||||
// INT_MAP[0] Register
|
||||
static const uint8_t BMI160_INT_MAP_FLAT_EN_MASK = (1 << 7);
|
||||
static const uint8_t BMI160_INT_MAP_ORIENTATION_EN_MASK = (1 << 6);
|
||||
static const uint8_t BMI160_INT_MAP_SINGLE_TAP_EN_MASK = (1 << 5);
|
||||
static const uint8_t BMI160_INT_MAP_DOUBLE_TAP_EN_MASK = (1 << 4);
|
||||
static const uint8_t BMI160_INT_MAP_NO_MOTION_EN_MASK = (1 << 3);
|
||||
static const uint8_t BMI160_INT_MAP_ANYMOTION_EN_MASK = (1 << 2);
|
||||
static const uint8_t BMI160_INT_MAP_HIGHG_EN_MASK = (1 << 1);
|
||||
static const uint8_t BMI160_INT_MAP_LOWG_MASK = (1 << 0);
|
||||
|
||||
// INT_MAP[1] Register
|
||||
static const uint8_t BMI160_INT_MAP_1_INT1_DATA_READY = (1 << 7);
|
||||
static const uint8_t BMI160_INT_MAP_1_INT1_FIFO_WATERMARK = (1 << 6);
|
||||
static const uint8_t BMI160_INT_MAP_1_INT1_FIFO_FULL = (1 << 5);
|
||||
static const uint8_t BMI160_INT_MAP_1_INT1_PMU_TRIGGER = (1 << 4);
|
||||
static const uint8_t BMI160_INT_MAP_1_INT2_DATA_READY = (1 << 3);
|
||||
static const uint8_t BMI160_INT_MAP_1_INT2_FIFO_WATERMARK = (1 << 2);
|
||||
static const uint8_t BMI160_INT_MAP_1_INT2_FIFO_FULL = (1 << 1);
|
||||
static const uint8_t BMI160_INT_MAP_1_INT2_PMU_TRIGGER = (1 << 0);
|
||||
|
||||
// INT_STATUS[0]
|
||||
static const uint8_t BMI160_INT_STATUS_0_FLAT_MASK = (1 << 7);
|
||||
static const uint8_t BMI160_INT_STATUS_0_ORIENT_MASK = (1 << 6);
|
||||
static const uint8_t BMI160_INT_STATUS_0_S_TAP_MASK = (1 << 5);
|
||||
static const uint8_t BMI160_INT_STATUS_0_D_TAP_MASK = (1 << 4);
|
||||
static const uint8_t BMI160_INT_STATUS_0_PMU_TRIGGER_MASK = (1 << 3);
|
||||
static const uint8_t BMI160_INT_STATUS_0_ANYM_MASK = (1 << 2);
|
||||
static const uint8_t BMI160_INT_STATUS_0_SIGMOT_MASK = (1 << 1);
|
||||
static const uint8_t BMI160_INT_STATUS_0_STEP_INT_MASK = (1 << 0);
|
||||
|
||||
// INT_STATUS[1]
|
||||
static const uint8_t BMI160_INT_STATUS_1_NOMO_MASK = (1 << 7);
|
||||
static const uint8_t BMI160_INT_STATUS_1_FWM_MASK = (1 << 6);
|
||||
static const uint8_t BMI160_INT_STATUS_1_FFULL_MASK = (1 << 5);
|
||||
static const uint8_t BMI160_INT_STATUS_1_DRDY_MASK = (1 << 4);
|
||||
static const uint8_t BMI160_INT_STATUS_1_LOWG_MASK = (1 << 3);
|
||||
static const uint8_t BMI160_INT_STATUS_1_HIGHG_Z_MASK = (1 << 2);
|
||||
// bit 1 & 0 reserved
|
||||
|
||||
// INT_STATUS[2]
|
||||
static const uint8_t BMI160_INT_STATUS_2_TAP_SIGN = (1 << 7);
|
||||
static const uint8_t BMI160_INT_STATUS_2_TAP_FIRST_Z = (1 << 6);
|
||||
static const uint8_t BMI160_INT_STATUS_2_TAP_FIRST_Y = (1 << 5);
|
||||
static const uint8_t BMI160_INT_STATUS_2_TAP_FIRST_X = (1 << 4);
|
||||
static const uint8_t BMI160_INT_STATUS_2_ANYM_SIGN = (1 << 3);
|
||||
static const uint8_t BMI160_INT_STATUS_2_ANYM_FIRST_Z = (1 << 2);
|
||||
static const uint8_t BMI160_INT_STATUS_2_ANYM_FIRST_Y = (1 << 1);
|
||||
static const uint8_t BMI160_INT_STATUS_2_ANYM_FIRST_X = (1 << 0);
|
||||
|
||||
// INT_EN[0]
|
||||
static const uint8_t BMI160_INT_EN_0_FLAT_EN = (1 << 7);
|
||||
static const uint8_t BMI160_INT_EN_0_ORIENT_EN = (1 << 6);
|
||||
static const uint8_t BMI160_INT_EN_0_S_TAP_EN = (1 << 5);
|
||||
static const uint8_t BMI160_INT_EN_0_D_TAP_EN = (1 << 4);
|
||||
// bit 3 reserved
|
||||
static const uint8_t BMI160_INT_EN_0_ANYMOTION_Z_EN = (1 << 2);
|
||||
static const uint8_t BMI160_INT_EN_0_ANYMOTION_Y_EN = (1 << 1);
|
||||
static const uint8_t BMI160_INT_EN_0_ANYMOTION_X_EN = (1 << 0);
|
||||
|
||||
// INT_EN[1]
|
||||
// bit 7 reserved
|
||||
static const uint8_t BMI160_INT_EN_1_FWM_EN = (1 << 6);
|
||||
static const uint8_t BMI160_INT_EN_1_FFUL_EN = (1 << 5);
|
||||
static const uint8_t BMI160_INT_EN_1_DRDY_EN = (1 << 4);
|
||||
static const uint8_t BMI160_INT_EN_1_LOW_EN = (1 << 3);
|
||||
static const uint8_t BMI160_INT_EN_1_HIGHZ_EN = (1 << 2);
|
||||
static const uint8_t BMI160_INT_EN_1_HIGHY_EN = (1 << 1);
|
||||
static const uint8_t BMI160_INT_EN_1_HIGHX_EN = (1 << 0);
|
||||
|
||||
// FIFO_CONFIG[0]
|
||||
static const uint8_t BMI160_FIFO_CONFIG_0_FWM_SHIFT = 0;
|
||||
static const uint8_t BMI160_FIFO_CONFIG_0_FWM_MASK = 0xff;
|
||||
|
||||
#define BMI160_FIFO_LEN_BYTES 1024
|
||||
#define BMI160_FIFO_WM_UNIT_BYTES 4
|
||||
|
||||
// FIFO_CONFIG[1]
|
||||
static const uint8_t BMI160_FIFO_CONFIG_1_GYR_EN = (1 << 7);
|
||||
static const uint8_t BMI160_FIFO_CONFIG_1_ACC_EN = (1 << 6);
|
||||
static const uint8_t BMI160_FIFO_CONFIG_1_MAG_EN = (1 << 5);
|
||||
static const uint8_t BMI160_FIFO_CONFIG_1_HDR_EN = (1 << 4);
|
||||
static const uint8_t BMI160_FIFO_CONFIG_1_TAG_INT1_EN = (1 << 3);
|
||||
static const uint8_t BMI160_FIFO_CONFIG_1_TAG_INT2_EN = (1 << 2);
|
||||
static const uint8_t BMI160_FIFO_CONFIG_1_TIME_EN = (1 << 1);
|
||||
// bit 0 reserved
|
||||
|
||||
// INT_MOTION[0]
|
||||
static const uint8_t BMI160_INT_MOTION_1_ANYM_DUR_SHIFT = 0;
|
||||
static const uint8_t BMI160_INT_MOTION_1_ANYM_DUR_MASK = 0x3;
|
||||
static const uint8_t BMI160_INT_MOTION_1_SLOWM_SHIFT = 2;
|
||||
static const uint8_t BMI160_INT_MOTION_1_SLOWM_MASK = 0xfc;
|
||||
|
||||
// INT_MOTION[1]
|
||||
static const uint8_t BMI160_INT_MOTION_1_ANYM_THRESH_SHIFT = 0;
|
||||
static const uint8_t BMI160_INT_MOTION_1_ANYM_THRESH_MASK = 0xff;
|
78
src/fw/drivers/imu/bmi160/bmi160_spi.c
Normal file
78
src/fw/drivers/imu/bmi160/bmi160_spi.c
Normal file
|
@ -0,0 +1,78 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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 <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "kernel/util/sleep.h"
|
||||
#include "drivers/spi.h"
|
||||
#include "board/board.h"
|
||||
|
||||
#include "bmi160_private.h"
|
||||
#include "bmi160_regs.h"
|
||||
|
||||
void bmi160_begin_burst(uint8_t addr) {
|
||||
spi_ll_slave_acquire(BMI160_SPI);
|
||||
spi_ll_slave_scs_assert(BMI160_SPI);
|
||||
spi_ll_slave_read_write(BMI160_SPI, addr);
|
||||
}
|
||||
|
||||
void bmi160_end_burst(void) {
|
||||
spi_ll_slave_scs_deassert(BMI160_SPI);
|
||||
spi_ll_slave_release(BMI160_SPI);
|
||||
}
|
||||
|
||||
uint8_t bmi160_read_reg(uint8_t reg) {
|
||||
uint8_t value;
|
||||
// Registers are read when the address MSB=1
|
||||
reg |= BMI160_READ_FLAG;
|
||||
SPIScatterGather sg_info[2] = {
|
||||
{.sg_len = 1, .sg_out = ®, .sg_in = NULL}, // address
|
||||
{.sg_len = 1, .sg_out = NULL, .sg_in = &value} // 8 bit register read
|
||||
};
|
||||
spi_slave_burst_read_write_scatter(BMI160_SPI, sg_info, ARRAY_LENGTH(sg_info));
|
||||
return value;
|
||||
}
|
||||
|
||||
uint16_t bmi160_read_16bit_reg(uint8_t reg) {
|
||||
// 16-bit registers are in little-endian format
|
||||
uint16_t value;
|
||||
reg |= BMI160_READ_FLAG;
|
||||
SPIScatterGather sg_info[2] = {
|
||||
{.sg_len = 1, .sg_out = ®, .sg_in = NULL}, // address
|
||||
{.sg_len = 2, .sg_out = NULL, .sg_in = &value} // 16 bit register read
|
||||
};
|
||||
spi_slave_burst_read_write_scatter(BMI160_SPI, sg_info, ARRAY_LENGTH(sg_info));
|
||||
return value;
|
||||
}
|
||||
|
||||
void bmi160_write_reg(uint8_t reg, uint8_t value) {
|
||||
reg &= BMI160_REG_MASK;
|
||||
SPIScatterGather sg_info[2] = {
|
||||
{.sg_len = 1, .sg_out = ®, .sg_in = NULL}, // address
|
||||
{.sg_len = 1, .sg_out = &value, .sg_in = NULL} // register write
|
||||
};
|
||||
spi_slave_burst_read_write_scatter(BMI160_SPI, sg_info, ARRAY_LENGTH(sg_info));
|
||||
}
|
||||
|
||||
void bmi160_enable_spi_mode(void) {
|
||||
// The BMI160 needs a rising edge on the SCS pin to switch into SPI mode.
|
||||
// The datasheet recommends performing a read to register 0x7F (reserved)
|
||||
// to put the chip into SPI mode.
|
||||
bmi160_read_reg(0x7f);
|
||||
|
||||
psleep(2); // Necessary on cold boots; not sure why
|
||||
}
|
37
src/fw/drivers/imu/imu_qemu.c
Normal file
37
src/fw/drivers/imu/imu_qemu.c
Normal file
|
@ -0,0 +1,37 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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 "drivers/imu.h"
|
||||
#include "drivers/imu/mag3110/mag3110.h"
|
||||
#include "drivers/mag.h"
|
||||
#include "drivers/qemu/qemu_accel.h"
|
||||
|
||||
void imu_init(void) {
|
||||
qemu_accel_init();
|
||||
#if CAPABILITY_HAS_MAGNETOMETER
|
||||
mag3110_init();
|
||||
#endif
|
||||
}
|
||||
|
||||
void imu_power_up(void) {
|
||||
}
|
||||
|
||||
void imu_power_down(void) {
|
||||
}
|
||||
|
||||
bool gyro_run_selftest(void) {
|
||||
return true;
|
||||
}
|
47
src/fw/drivers/imu/imu_silk.c
Normal file
47
src/fw/drivers/imu/imu_silk.c
Normal file
|
@ -0,0 +1,47 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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 "drivers/accel.h"
|
||||
#include "drivers/imu.h"
|
||||
#include "drivers/imu/bma255/bma255.h"
|
||||
|
||||
void imu_init(void) {
|
||||
bma255_init();
|
||||
}
|
||||
|
||||
void imu_power_up(void) {
|
||||
// NYI
|
||||
}
|
||||
|
||||
void imu_power_down(void) {
|
||||
// NYI
|
||||
}
|
||||
|
||||
#if !TARGET_QEMU
|
||||
|
||||
////////////////////////////////////
|
||||
// Accel
|
||||
//
|
||||
////////////////////////////////////
|
||||
|
||||
void accel_enable_double_tap_detection(bool on) {
|
||||
}
|
||||
|
||||
bool accel_get_double_tap_detection_enabled(void) {
|
||||
return false;
|
||||
}
|
||||
|
||||
#endif /* !TARGET_QEMU */
|
60
src/fw/drivers/imu/imu_snowy_evt.c
Normal file
60
src/fw/drivers/imu/imu_snowy_evt.c
Normal file
|
@ -0,0 +1,60 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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 "system/logging.h"
|
||||
#include "drivers/imu/bmi160/bmi160.h"
|
||||
#include "drivers/mag.h"
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
void imu_init(void) {
|
||||
bmi160_init();
|
||||
bmi160_set_accel_power_mode(BMI160_Accel_Mode_Normal);
|
||||
}
|
||||
|
||||
void imu_power_up(void) {
|
||||
// Unused in snowy as PMIC turns on everything
|
||||
}
|
||||
|
||||
void imu_power_down(void) {
|
||||
// Unused in snowy as PMIC turns off everything
|
||||
}
|
||||
|
||||
// We don't actually support the mag at all on snowy_evt, as we tried a more complicated
|
||||
// arrangement where the mag was a slave of the bmi160 chip. We never fully implemented it, and
|
||||
// as we abandoned that approach there's no point in ever implementing it. Below are a bunch of
|
||||
// stubs that do nothing on this board.
|
||||
|
||||
void mag_use(void) {
|
||||
}
|
||||
|
||||
void mag_start_sampling(void) {
|
||||
}
|
||||
|
||||
void mag_release(void) {
|
||||
}
|
||||
|
||||
MagReadStatus mag_read_data(MagData *data) {
|
||||
return MagReadCommunicationFail;
|
||||
}
|
||||
|
||||
bool mag_change_sample_rate(MagSampleRate rate) {
|
||||
return false;
|
||||
}
|
||||
|
40
src/fw/drivers/imu/imu_snowy_evt2.c
Normal file
40
src/fw/drivers/imu/imu_snowy_evt2.c
Normal file
|
@ -0,0 +1,40 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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 "drivers/imu/bmi160/bmi160.h"
|
||||
#include "drivers/imu/mag3110/mag3110.h"
|
||||
|
||||
#include "system/logging.h"
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
void imu_init(void) {
|
||||
bmi160_init();
|
||||
bmi160_set_accel_power_mode(BMI160_Accel_Mode_Normal);
|
||||
|
||||
mag3110_init();
|
||||
}
|
||||
|
||||
void imu_power_up(void) {
|
||||
// Unused in snowy as PMIC turns on everything
|
||||
}
|
||||
|
||||
void imu_power_down(void) {
|
||||
// Unused in snowy as PMIC turns off everything
|
||||
}
|
39
src/fw/drivers/imu/imu_tintin.c
Normal file
39
src/fw/drivers/imu/imu_tintin.c
Normal file
|
@ -0,0 +1,39 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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 <inttypes.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "system/logging.h"
|
||||
#include "drivers/imu/lis3dh/lis3dh.h"
|
||||
#include "drivers/imu/mag3110/mag3110.h"
|
||||
|
||||
|
||||
void imu_init(void) {
|
||||
// Init accelerometer
|
||||
lis3dh_init();
|
||||
// Init magnetometer
|
||||
mag3110_init();
|
||||
}
|
||||
|
||||
void imu_power_up(void) {
|
||||
lis3dh_power_up();
|
||||
}
|
||||
|
||||
void imu_power_down(void) {
|
||||
lis3dh_power_down();
|
||||
}
|
390
src/fw/drivers/imu/lis3dh/config.c
Normal file
390
src/fw/drivers/imu/lis3dh/config.c
Normal file
|
@ -0,0 +1,390 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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 "lis3dh.h"
|
||||
#include "registers.h"
|
||||
|
||||
#include "drivers/i2c.h"
|
||||
#include "drivers/legacy/accel.h"
|
||||
#include "system/logging.h"
|
||||
#include "util/math.h"
|
||||
#include "util/size.h"
|
||||
|
||||
//! @file accel_config.c
|
||||
//! procedures for dealing with I2C communication with the accel
|
||||
|
||||
struct I2CCommand {
|
||||
uint8_t register_address;
|
||||
uint8_t value;
|
||||
};
|
||||
|
||||
//
|
||||
// Boiler plate functions for talking over i2c
|
||||
//
|
||||
|
||||
static uint8_t prv_read_reg(uint8_t address) {
|
||||
uint8_t reg;
|
||||
lis3dh_read(address, 1, ®);
|
||||
|
||||
return (reg);
|
||||
}
|
||||
|
||||
static bool prv_write_reg(uint8_t address, uint8_t value) {
|
||||
return (lis3dh_write(address, 1, &value));
|
||||
}
|
||||
|
||||
static bool send_i2c_commands(struct I2CCommand* commands, int num_commands) {
|
||||
for (int i = 0; i < num_commands; ++i) {
|
||||
bool result = prv_write_reg(commands[i].register_address, commands[i].value);
|
||||
|
||||
if (!result) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void lis3dh_enable_fifo(void) {
|
||||
uint8_t ctrl_reg5 = prv_read_reg(LIS3DH_CTRL_REG5);
|
||||
ctrl_reg5 |= FIFO_EN;
|
||||
prv_write_reg(LIS3DH_CTRL_REG5, ctrl_reg5);
|
||||
}
|
||||
|
||||
void lis3dh_disable_fifo(void) {
|
||||
uint8_t ctrl_reg5 = prv_read_reg(LIS3DH_CTRL_REG5);
|
||||
ctrl_reg5 &= ~FIFO_EN;
|
||||
prv_write_reg(LIS3DH_CTRL_REG5, ctrl_reg5);
|
||||
}
|
||||
|
||||
bool lis3dh_is_fifo_enabled(void) {
|
||||
uint8_t ctrl_reg5 = prv_read_reg(LIS3DH_CTRL_REG5);
|
||||
return (ctrl_reg5 & FIFO_EN);
|
||||
}
|
||||
|
||||
void lis3dh_disable_click(void) {
|
||||
uint8_t ctrl_reg3 = prv_read_reg(LIS3DH_CTRL_REG3);
|
||||
ctrl_reg3 &= ~I1_CLICK;
|
||||
prv_write_reg(LIS3DH_CTRL_REG3, ctrl_reg3);
|
||||
}
|
||||
|
||||
void lis3dh_enable_click(void) {
|
||||
uint8_t ctrl_reg3 = prv_read_reg(LIS3DH_CTRL_REG3);
|
||||
ctrl_reg3 |= I1_CLICK;
|
||||
prv_write_reg(LIS3DH_CTRL_REG3, ctrl_reg3);
|
||||
}
|
||||
|
||||
//
|
||||
// Accel config Getter/Setters
|
||||
//
|
||||
|
||||
void lis3dh_set_interrupt_axis(AccelAxisType axis, bool double_click) {
|
||||
// get the current state of the registers
|
||||
uint8_t reg_1 = prv_read_reg(LIS3DH_CTRL_REG1);
|
||||
|
||||
// clear the axis-enable bits
|
||||
reg_1 = reg_1 & ~(0x1 | 0x2 | 0x4);
|
||||
uint8_t click_cfg = 0;
|
||||
|
||||
switch (axis) {
|
||||
case ACCEL_AXIS_X:
|
||||
reg_1 |= 0x01;
|
||||
click_cfg = 0x01;
|
||||
break;
|
||||
case ACCEL_AXIS_Y:
|
||||
reg_1 |= 0x02;
|
||||
click_cfg = 0x04;
|
||||
break;
|
||||
case ACCEL_AXIS_Z:
|
||||
reg_1 |= 0x04;
|
||||
click_cfg = 0x10;
|
||||
break;
|
||||
default:
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Unknown axis");
|
||||
}
|
||||
|
||||
if (double_click) {
|
||||
click_cfg <<= 1;
|
||||
}
|
||||
|
||||
if ((!prv_write_reg(LIS3DH_CTRL_REG1, reg_1))
|
||||
|| (!prv_write_reg(LIS3DH_CLICK_CFG, click_cfg))) {
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Failed to write axis selection");
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t lis3dh_get_click_window() {
|
||||
return prv_read_reg(LIS3DH_TIME_WINDOW);
|
||||
}
|
||||
void lis3dh_set_click_window(uint8_t window) {
|
||||
if (!prv_write_reg(LIS3DH_TIME_WINDOW, MIN(window, LIS3DH_MAX_CLICK_WINDOW))) {
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Failed to write click latency");
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t lis3dh_get_click_latency() {
|
||||
return prv_read_reg(LIS3DH_TIME_LATENCY);
|
||||
}
|
||||
void lis3dh_set_click_latency(uint8_t latency) {
|
||||
if (!prv_write_reg(LIS3DH_TIME_LATENCY, MIN(latency, LIS3DH_MAX_CLICK_LATENCY))) {
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Failed to write click latency");
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t lis3dh_get_interrupt_threshold() {
|
||||
return prv_read_reg(LIS3DH_CLICK_THS);
|
||||
}
|
||||
void lis3dh_set_interrupt_threshold(uint8_t threshold) {
|
||||
if (!prv_write_reg(LIS3DH_CLICK_THS, MIN(threshold, LIS3DH_MAX_THRESHOLD))) {
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Failed to set interrupt threshold");
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t lis3dh_get_interrupt_time_limit() {
|
||||
return prv_read_reg(LIS3DH_TIME_LIMIT);
|
||||
}
|
||||
void lis3dh_set_interrupt_time_limit(uint8_t time_limit) {
|
||||
if (!prv_write_reg(LIS3DH_TIME_LIMIT, MIN(time_limit, LIS3DH_MAX_TIME_LIMIT))) {
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Failed to set interrupt time limit");
|
||||
}
|
||||
}
|
||||
|
||||
bool lis3dh_set_fifo_wtm(uint8_t wtm) {
|
||||
uint8_t fifo_ctrl_reg = prv_read_reg(LIS3DH_FIFO_CTRL_REG);
|
||||
fifo_ctrl_reg &= ~THR_MASK;
|
||||
fifo_ctrl_reg |= (wtm & THR_MASK);
|
||||
|
||||
return (prv_write_reg(LIS3DH_FIFO_CTRL_REG, fifo_ctrl_reg));
|
||||
}
|
||||
|
||||
uint8_t lis3dh_get_fifo_wtm(void) {
|
||||
uint8_t fifo_ctrl_reg = prv_read_reg(LIS3DH_FIFO_CTRL_REG);
|
||||
return (fifo_ctrl_reg & THR_MASK);
|
||||
}
|
||||
|
||||
AccelSamplingRate accel_get_sampling_rate(void) {
|
||||
uint8_t odr = ODR_MASK & prv_read_reg(LIS3DH_CTRL_REG1);
|
||||
|
||||
if (odr == (ODR2 | ODR0)) {
|
||||
return (ACCEL_SAMPLING_100HZ);
|
||||
} else if (odr == ODR2) {
|
||||
return (ACCEL_SAMPLING_50HZ);
|
||||
} else if (odr == (ODR1 | ODR0)) {
|
||||
return (ACCEL_SAMPLING_25HZ);
|
||||
} else if (odr == (ODR1)) {
|
||||
return (ACCEL_SAMPLING_10HZ);
|
||||
} else {
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Unrecognized ODR value %d", odr);
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
|
||||
bool accel_set_sampling_rate(AccelSamplingRate rate) {
|
||||
uint8_t odr;
|
||||
|
||||
switch (rate) {
|
||||
case ACCEL_SAMPLING_100HZ:
|
||||
odr = ODR2 | ODR0;
|
||||
break;
|
||||
case ACCEL_SAMPLING_50HZ:
|
||||
odr = ODR2;
|
||||
break;
|
||||
case ACCEL_SAMPLING_25HZ:
|
||||
odr = ODR1 | ODR0;
|
||||
break;
|
||||
case ACCEL_SAMPLING_10HZ:
|
||||
odr = ODR1;
|
||||
break;
|
||||
default:
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Unsupported sampling rate %d", rate);
|
||||
return (false);
|
||||
}
|
||||
|
||||
uint8_t ctrl_reg_1 = prv_read_reg(LIS3DH_CTRL_REG1);
|
||||
ctrl_reg_1 &= ~ODR_MASK;
|
||||
ctrl_reg_1 |= (odr & ODR_MASK);
|
||||
//TODO: fix hack below (enabling axes after lis3dh_power_down)
|
||||
ctrl_reg_1 |= (Xen | Yen | Zen); //enable x, y and z axis
|
||||
bool res = prv_write_reg(LIS3DH_CTRL_REG1, ctrl_reg_1);
|
||||
|
||||
// Update the click limit based on sampling frequency
|
||||
uint8_t time_limit = rate * LIS3DH_TIME_LIMIT_MULT / LIS3DH_TIME_LIMIT_DIV;
|
||||
lis3dh_set_interrupt_time_limit(time_limit);
|
||||
PBL_LOG(LOG_LEVEL_DEBUG, "setting click time limit to 0x%x",
|
||||
lis3dh_get_interrupt_time_limit());
|
||||
|
||||
// Update click latency
|
||||
uint8_t time_latency = rate * LIS3DH_TIME_LATENCY_MULT / LIS3DH_TIME_LATENCY_DIV;
|
||||
lis3dh_set_click_latency(time_latency);
|
||||
PBL_LOG(LOG_LEVEL_DEBUG, "setting click time latency to 0x%x",
|
||||
lis3dh_get_click_latency());
|
||||
|
||||
// Update click window
|
||||
uint32_t time_window = rate * LIS3DH_TIME_WINDOW_MULT / LIS3DH_TIME_WINDOW_DIV;
|
||||
time_window = MIN(time_window, 0xff);
|
||||
lis3dh_set_click_window(time_window);
|
||||
PBL_LOG(LOG_LEVEL_DEBUG, "setting click time window to 0x%x",
|
||||
lis3dh_get_click_window());
|
||||
|
||||
|
||||
return (res);
|
||||
}
|
||||
|
||||
Lis3dhScale accel_get_scale(void) {
|
||||
uint8_t fs = FS_MASK & prv_read_reg(LIS3DH_CTRL_REG4);
|
||||
|
||||
if (fs == (FS0 | FS1)) {
|
||||
return (LIS3DH_SCALE_16G);
|
||||
} else if (fs == FS1) {
|
||||
return (LIS3DH_SCALE_8G);
|
||||
} else if (fs == FS0) {
|
||||
return (LIS3DH_SCALE_4G);
|
||||
} else if (fs == 0) {
|
||||
return (LIS3DH_SCALE_2G);
|
||||
} else {
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Unrecognized FS value %d", fs);
|
||||
return (LIS3DH_SCALE_UNKNOWN);
|
||||
}
|
||||
}
|
||||
|
||||
bool accel_set_scale(Lis3dhScale scale) {
|
||||
uint8_t fs;
|
||||
|
||||
switch (scale) {
|
||||
case LIS3DH_SCALE_16G:
|
||||
fs = (FS0 | FS1);
|
||||
break;
|
||||
case LIS3DH_SCALE_8G:
|
||||
fs = FS1;
|
||||
break;
|
||||
case LIS3DH_SCALE_4G:
|
||||
fs = FS0;
|
||||
break;
|
||||
case LIS3DH_SCALE_2G:
|
||||
fs = 0;
|
||||
break;
|
||||
default:
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Unsupported scale %d", scale);
|
||||
return (false);
|
||||
}
|
||||
|
||||
uint8_t ctrl_reg_4 = prv_read_reg(LIS3DH_CTRL_REG4);
|
||||
ctrl_reg_4 &= ~FS_MASK;
|
||||
ctrl_reg_4 |= (fs & FS_MASK);
|
||||
bool res = prv_write_reg(LIS3DH_CTRL_REG4, ctrl_reg_4);
|
||||
|
||||
lis3dh_set_interrupt_threshold(scale * LIS3DH_THRESHOLD_MULT
|
||||
/ LIS3DH_THRESHOLD_DIV);
|
||||
PBL_LOG(LOG_LEVEL_DEBUG, "setting click threshold to 0x%x",
|
||||
lis3dh_get_interrupt_threshold());
|
||||
return (res);
|
||||
}
|
||||
|
||||
bool lis3dh_set_fifo_mode(uint8_t mode) {
|
||||
uint8_t fifo_ctrl_reg = prv_read_reg(LIS3DH_FIFO_CTRL_REG);
|
||||
fifo_ctrl_reg &= ~MODE_MASK;
|
||||
fifo_ctrl_reg |= (mode & MODE_MASK);
|
||||
return (prv_write_reg(LIS3DH_FIFO_CTRL_REG, fifo_ctrl_reg));
|
||||
}
|
||||
|
||||
uint8_t lis3dh_get_fifo_mode(void) {
|
||||
uint8_t fifo_ctrl_reg = prv_read_reg(LIS3DH_FIFO_CTRL_REG);
|
||||
return (fifo_ctrl_reg & MODE_MASK);
|
||||
}
|
||||
|
||||
//! Configure the accel to run "Self Test 0". See S3.2.2 of the accel datasheet for more information
|
||||
bool lis3dh_enter_self_test_mode(SelfTestMode mode) {
|
||||
uint8_t reg4 = 0x8;
|
||||
|
||||
switch (mode) {
|
||||
case SELF_TEST_MODE_ONE:
|
||||
reg4 |= 0x2;
|
||||
break;
|
||||
case SELF_TEST_MODE_TWO:
|
||||
reg4 |= (0x2 | 0x4);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
struct I2CCommand test_mode_config[] = {
|
||||
{ LIS3DH_CTRL_REG1, 0x9f },
|
||||
{ LIS3DH_CTRL_REG3, 0x00 },
|
||||
{ LIS3DH_CTRL_REG4, reg4 }
|
||||
};
|
||||
|
||||
return send_i2c_commands(test_mode_config, ARRAY_LENGTH(test_mode_config));
|
||||
}
|
||||
|
||||
void lis3dh_exit_self_test_mode(void) {
|
||||
lis3dh_config_set_defaults();
|
||||
}
|
||||
|
||||
//
|
||||
// Boot-time config
|
||||
//
|
||||
|
||||
//! Ask the accel for a 8-bit value that's programmed into the IC at the
|
||||
//! factory. Useful as a sanity check to make sure everything came up properly.
|
||||
bool lis3dh_sanity_check(void) {
|
||||
uint8_t whoami = prv_read_reg(LIS3DH_WHO_AM_I);
|
||||
PBL_LOG(LOG_LEVEL_DEBUG, "Read accel whomai byte 0x%x, expecting 0x%x", whoami, LIS3DH_WHOAMI_BYTE);
|
||||
return (whoami == LIS3DH_WHOAMI_BYTE);
|
||||
}
|
||||
|
||||
bool lis3dh_config_set_defaults() {
|
||||
// Follow the startup sequence from AN3308
|
||||
struct I2CCommand accel_init_commands[] = {
|
||||
{ LIS3DH_CTRL_REG1, (ODR1 | ODR0 | Zen | Yen | Xen) }, // 25MHz, Enable X,Y,Z Axes
|
||||
{ LIS3DH_CTRL_REG2, 0x00 },
|
||||
{ LIS3DH_CTRL_REG3, I1_WTM }, // FIFO Watermark on INT1
|
||||
{ LIS3DH_CTRL_REG4, (BDU | FS0 | HR) }, // Block Read, +/- 4g sensitivity
|
||||
{ LIS3DH_CTRL_REG6, I2_CLICK }, // Click on INT2
|
||||
|
||||
{ LIS3DH_INT1_THS, 0x20 }, // intertial threshold (MAX 0x7f)
|
||||
{ LIS3DH_INT1_DURATION, 0x10 }, // interrupt duration (units of 1/(update frequency) [See CTRL_REG1])
|
||||
{ LIS3DH_INT1_CFG, 0x00 }, // no inertial interrupts
|
||||
|
||||
// click threshold (MAX 0x7f)
|
||||
{ LIS3DH_CLICK_THS, LIS3DH_SCALE_4G * LIS3DH_THRESHOLD_MULT
|
||||
/ LIS3DH_THRESHOLD_DIV },
|
||||
|
||||
// click time limit (units of 1/(update frequency) [See CTRL_REG1])
|
||||
{ LIS3DH_TIME_LIMIT, ACCEL_DEFAULT_SAMPLING_RATE * LIS3DH_TIME_LIMIT_MULT
|
||||
/ LIS3DH_TIME_LIMIT_DIV},
|
||||
|
||||
{ LIS3DH_CLICK_CFG, (XS | YS | ZS) }, // single click detection on the X axis
|
||||
|
||||
{ LIS3DH_FIFO_CTRL_REG, MODE_BYPASS | 0x19 }, // BYPASS MODE and 25 samples per interrupt
|
||||
|
||||
// time latency, ie "debounce time" after the first of a double click
|
||||
// (units of 1/(update frequency) [See CTRL_REG1])
|
||||
{ LIS3DH_TIME_LATENCY, ACCEL_DEFAULT_SAMPLING_RATE * LIS3DH_TIME_LATENCY_MULT
|
||||
/ LIS3DH_TIME_LATENCY_DIV },
|
||||
|
||||
// max time allowed between clicks for a double click (end to start)
|
||||
// (units of 1/(update frequency) [See CTRL_REG1])
|
||||
{ LIS3DH_TIME_WINDOW, ACCEL_DEFAULT_SAMPLING_RATE * LIS3DH_TIME_WINDOW_MULT
|
||||
/ LIS3DH_TIME_WINDOW_DIV}
|
||||
};
|
||||
|
||||
if (!send_i2c_commands(accel_init_commands, ARRAY_LENGTH(accel_init_commands))) {
|
||||
accel_stop();
|
||||
PBL_LOG(LOG_LEVEL_WARNING, "Failed to initialize accelerometer");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
657
src/fw/drivers/imu/lis3dh/lis3dh.c
Normal file
657
src/fw/drivers/imu/lis3dh/lis3dh.c
Normal file
|
@ -0,0 +1,657 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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 "lis3dh.h"
|
||||
|
||||
#include "board/board.h"
|
||||
#include "drivers/exti.h"
|
||||
#include "drivers/gpio.h"
|
||||
#include "drivers/i2c.h"
|
||||
#include "drivers/legacy/accel.h"
|
||||
#include "drivers/periph_config.h"
|
||||
#include "drivers/vibe.h"
|
||||
#include "kernel/events.h"
|
||||
#include "pebble_errors.h"
|
||||
#include "registers.h"
|
||||
#include "services/common/accel_manager.h"
|
||||
#include "services/common/analytics/analytics.h"
|
||||
#include "services/common/vibe_pattern.h"
|
||||
#include "services/imu/units.h"
|
||||
#include "system/logging.h"
|
||||
#include "os/mutex.h"
|
||||
#include "system/passert.h"
|
||||
#include "util/size.h"
|
||||
#include "kernel/util/sleep.h"
|
||||
|
||||
#define STM32F2_COMPATIBLE
|
||||
#define STM32F4_COMPATIBLE
|
||||
#include <mcu.h>
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#define ACCEL_MAX_IDLE_DELTA 100
|
||||
|
||||
// State
|
||||
static bool s_initialized = false;
|
||||
static bool s_running = false;
|
||||
static const Lis3dhScale s_accel_scale = LIS3DH_SCALE_4G;
|
||||
static PebbleMutex * s_accel_mutex;
|
||||
static uint64_t s_latest_timestamp;
|
||||
static uint8_t s_pending_accel_event = false;
|
||||
static bool s_is_idle = false;
|
||||
static AccelRawData s_last_analytics_position;
|
||||
static AccelRawData s_latest_reading;
|
||||
|
||||
// Buffer for holding the accel data
|
||||
static SharedCircularBuffer s_buffer;
|
||||
static uint8_t s_buffer_storage[50*sizeof(AccelRawData)]; // 400 bytes (~1s of data at 50Hz)
|
||||
|
||||
static void lis3dh_IRQ1_handler(bool *should_context_switch);
|
||||
static void lis3dh_IRQ2_handler(bool *should_context_switch);
|
||||
|
||||
static void prv_accel_configure_interrupts(void) {
|
||||
exti_configure_pin(BOARD_CONFIG_ACCEL.accel_ints[0], ExtiTrigger_Rising, lis3dh_IRQ1_handler);
|
||||
exti_configure_pin(BOARD_CONFIG_ACCEL.accel_ints[1], ExtiTrigger_Rising, lis3dh_IRQ2_handler);
|
||||
}
|
||||
|
||||
static void disable_accel_interrupts(void) {
|
||||
for (unsigned int i = 0; i < ARRAY_LENGTH(BOARD_CONFIG_ACCEL.accel_ints); i++) {
|
||||
exti_disable(BOARD_CONFIG_ACCEL.accel_ints[i]);
|
||||
}
|
||||
}
|
||||
|
||||
static void enable_accel_interrupts(void) {
|
||||
for (unsigned int i = 0; i < ARRAY_LENGTH(BOARD_CONFIG_ACCEL.accel_ints); i++) {
|
||||
exti_enable(BOARD_CONFIG_ACCEL.accel_ints[i]);
|
||||
}
|
||||
}
|
||||
|
||||
static void clear_accel_interrupts(void) {
|
||||
for (unsigned int i = 0; i < ARRAY_LENGTH(BOARD_CONFIG_ACCEL.accel_ints); i++) {
|
||||
EXTI_ClearFlag(BOARD_CONFIG_ACCEL.accel_ints[i].exti_line);
|
||||
EXTI_ClearITPendingBit(BOARD_CONFIG_ACCEL.accel_ints[i].exti_line);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static int16_t raw_to_mgs(int16_t raw, Lis3dhScale scale) {
|
||||
int16_t mgs_per_digit;
|
||||
switch (scale) {
|
||||
case LIS3DH_SCALE_2G:
|
||||
mgs_per_digit = 1;
|
||||
break;
|
||||
case LIS3DH_SCALE_4G:
|
||||
mgs_per_digit = 2;
|
||||
break;
|
||||
case LIS3DH_SCALE_8G:
|
||||
mgs_per_digit = 4;
|
||||
break;
|
||||
case LIS3DH_SCALE_16G:
|
||||
mgs_per_digit = 12;
|
||||
break;
|
||||
default:
|
||||
WTF;
|
||||
}
|
||||
|
||||
// least significant 4 bits need to be removed
|
||||
return ((raw >> 4) * mgs_per_digit);
|
||||
}
|
||||
|
||||
static int16_t get_axis_data(AccelAxisType axis, uint8_t *raw_data) {
|
||||
// each sample is 2 bytes for each axis
|
||||
int offset = 2 * BOARD_CONFIG_ACCEL.accel_config.axes_offsets[axis];
|
||||
int invert = BOARD_CONFIG_ACCEL.accel_config.axes_inverts[axis];
|
||||
int16_t raw = (((int16_t)raw_data[offset + 1]) << 8) | raw_data[offset];
|
||||
int16_t converted = (invert ? -1 : 1) * raw_to_mgs(raw, s_accel_scale);
|
||||
return (converted);
|
||||
}
|
||||
|
||||
// Simple read register command with no error handling
|
||||
static bool prv_read_register(uint8_t register_address, uint8_t *result) {
|
||||
return i2c_read_register(I2C_LIS3DH, register_address, result);
|
||||
}
|
||||
|
||||
// Simple write register command with no error handling
|
||||
static bool prv_write_register(uint8_t register_address, uint8_t value) {
|
||||
return i2c_write_register(I2C_LIS3DH, register_address, value);
|
||||
}
|
||||
|
||||
static void prv_clear_fifo(void) {
|
||||
// Use I2C calls instead of accel wrappers to avoid recursion (reset called from lis3dh_read/accel_write)
|
||||
uint8_t mode;
|
||||
if (!prv_read_register(LIS3DH_FIFO_CTRL_REG, &mode)) {
|
||||
return;
|
||||
}
|
||||
if (mode != MODE_BYPASS) {
|
||||
uint8_t fifo_ctrl_reg = mode & ~MODE_MASK;
|
||||
fifo_ctrl_reg |= (MODE_BYPASS & MODE_MASK);
|
||||
if (!prv_write_register(LIS3DH_FIFO_CTRL_REG, fifo_ctrl_reg)) {
|
||||
return;
|
||||
}
|
||||
if (!prv_write_register(LIS3DH_FIFO_CTRL_REG, mode)) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static void prv_reset(void) {
|
||||
lis3dh_lock();
|
||||
if (i2c_bitbang_recovery(I2C_LIS3DH)) {
|
||||
prv_clear_fifo();
|
||||
}
|
||||
lis3dh_unlock();
|
||||
analytics_inc(ANALYTICS_DEVICE_METRIC_ACCEL_RESET_COUNT, AnalyticsClient_System);
|
||||
}
|
||||
|
||||
bool lis3dh_read(uint8_t register_address, uint8_t read_size, uint8_t *buffer) {
|
||||
bool running = s_running;
|
||||
|
||||
if (!running) {
|
||||
if (!accel_start()) {
|
||||
// couldn't start the accel
|
||||
return (false);
|
||||
}
|
||||
}
|
||||
|
||||
if (!i2c_read_register_block(I2C_LIS3DH, register_address, read_size, buffer)) {
|
||||
prv_reset();
|
||||
return (false);
|
||||
}
|
||||
|
||||
if (!running) {
|
||||
accel_stop();
|
||||
}
|
||||
|
||||
return (true);
|
||||
}
|
||||
|
||||
bool lis3dh_write(uint8_t address, uint8_t write_size, const uint8_t *buffer) {
|
||||
bool running = accel_running();
|
||||
|
||||
if (!running) {
|
||||
if (!accel_start()) {
|
||||
// couldn't start the accel
|
||||
return (false);
|
||||
}
|
||||
}
|
||||
|
||||
if (!i2c_write_register_block(I2C_LIS3DH, address, write_size, buffer)) {
|
||||
prv_reset();
|
||||
return (false);
|
||||
}
|
||||
|
||||
if (!running) {
|
||||
accel_stop();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
AccelRawData s_accel_data;
|
||||
void accel_get_last_data(AccelRawData* data) {
|
||||
*data = s_accel_data;
|
||||
}
|
||||
|
||||
void accel_get_data(AccelRawData* data, int num_samples) {
|
||||
if (!s_running) {
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Accel Not Running");
|
||||
return;
|
||||
}
|
||||
|
||||
// accel output registers have adjacent addresses
|
||||
// MSB enables address auto-increment
|
||||
int num_bytes = 6 * num_samples;
|
||||
|
||||
// Overflow bit doesn't get cleared until number of samples in fifo goes below
|
||||
// the watermark. Read an extra item from the fifo and just throw it away.
|
||||
int read_num_bytes = num_bytes + 6;
|
||||
|
||||
uint8_t start_addr = 1 << 7 | LIS3DH_OUT_X_L;
|
||||
uint8_t buffer[read_num_bytes];
|
||||
lis3dh_read(start_addr, read_num_bytes, buffer);
|
||||
for (uint8_t *ptr = buffer; ptr < (buffer + num_bytes); ptr+=6) {
|
||||
data->x = get_axis_data(ACCEL_AXIS_X, ptr);
|
||||
data->y = get_axis_data(ACCEL_AXIS_Y, ptr);
|
||||
data->z = get_axis_data(ACCEL_AXIS_Z, ptr);
|
||||
s_accel_data = *data;
|
||||
data++;
|
||||
}
|
||||
}
|
||||
|
||||
void lis3dh_init(void) {
|
||||
PBL_ASSERTN(!s_initialized);
|
||||
|
||||
lis3dh_init_mutex();
|
||||
|
||||
s_initialized = true;
|
||||
|
||||
if (!accel_start()) {
|
||||
s_initialized = false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!lis3dh_config_set_defaults()) {
|
||||
// accel write will call reset if it fails, so just try again
|
||||
if (!lis3dh_config_set_defaults()) {
|
||||
s_initialized = false;
|
||||
return;
|
||||
}
|
||||
}
|
||||
shared_circular_buffer_init(&s_buffer, s_buffer_storage, sizeof(s_buffer_storage));
|
||||
|
||||
// Test out the peripheral real quick
|
||||
if (!lis3dh_sanity_check()) {
|
||||
s_initialized = false;
|
||||
return;
|
||||
}
|
||||
|
||||
accel_stop();
|
||||
|
||||
prv_accel_configure_interrupts();
|
||||
}
|
||||
|
||||
void lis3dh_power_up(void) {
|
||||
if (accel_start()) {
|
||||
uint8_t ctrl_reg1;
|
||||
if (prv_read_register(LIS3DH_CTRL_REG1, &ctrl_reg1)) {
|
||||
ctrl_reg1 &= ~LPen;
|
||||
if (prv_write_register(LIS3DH_CTRL_REG1, ctrl_reg1)) {
|
||||
// Write successful, low power mode disabled
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Failed to exit low power mode");
|
||||
}
|
||||
|
||||
void lis3dh_power_down(void) {
|
||||
if (accel_start()) {
|
||||
uint8_t ctrl_reg1;
|
||||
if (prv_read_register(LIS3DH_CTRL_REG1, &ctrl_reg1)) {
|
||||
ctrl_reg1 |= LPen;
|
||||
if (prv_write_register(LIS3DH_CTRL_REG1, ctrl_reg1)) {
|
||||
// Write successful, low power mode enabled
|
||||
accel_stop();
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Failed to enter low power mode");
|
||||
}
|
||||
|
||||
bool accel_running(void) {
|
||||
return (s_running);
|
||||
}
|
||||
|
||||
bool accel_start(void) {
|
||||
if (!s_initialized) {
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Failed to start accel, not yet initialized");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (s_running) {
|
||||
return true; // Already running
|
||||
}
|
||||
|
||||
i2c_use(I2C_LIS3DH);
|
||||
|
||||
s_running = true;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void accel_stop(void) {
|
||||
PBL_ASSERTN(s_initialized);
|
||||
if (s_running) {
|
||||
disable_accel_interrupts();
|
||||
clear_accel_interrupts();
|
||||
|
||||
i2c_release(I2C_LIS3DH);
|
||||
|
||||
enable_accel_interrupts();
|
||||
|
||||
s_running = false;
|
||||
}
|
||||
}
|
||||
|
||||
void lis3dh_init_mutex(void) {
|
||||
s_accel_mutex = mutex_create();
|
||||
}
|
||||
|
||||
void lis3dh_lock(void) {
|
||||
mutex_lock(s_accel_mutex);
|
||||
}
|
||||
|
||||
void lis3dh_unlock(void) {
|
||||
mutex_unlock(s_accel_mutex);
|
||||
}
|
||||
|
||||
static void prv_handle_tap(void *data) {
|
||||
IMUCoordinateAxis axis;
|
||||
int direction;
|
||||
|
||||
if (s_running) {
|
||||
uint8_t click_src;
|
||||
lis3dh_read(LIS3DH_CLICK_SRC, 1, &click_src);
|
||||
|
||||
if (click_src & (1 << BOARD_CONFIG_ACCEL.accel_config.axes_offsets[AXIS_X])) {
|
||||
axis = AXIS_X;
|
||||
} else if (click_src & (1 << BOARD_CONFIG_ACCEL.accel_config.axes_offsets[AXIS_Y])) {
|
||||
axis = AXIS_Y;
|
||||
} else if (click_src & (1 << BOARD_CONFIG_ACCEL.accel_config.axes_offsets[AXIS_Z])) {
|
||||
axis = AXIS_Z;
|
||||
} else {
|
||||
// something has reset the register, ignore
|
||||
return;
|
||||
}
|
||||
// sign bit is zero if positive, 1 if negative
|
||||
direction = (click_src & Sign) ? -1 : 1;
|
||||
} else {
|
||||
// when no-one has subscribed, we only listen to the x axis
|
||||
axis = AXIS_X;
|
||||
// no sign info
|
||||
direction = 0;
|
||||
}
|
||||
|
||||
PebbleEvent e = {
|
||||
.type = PEBBLE_ACCEL_SHAKE_EVENT,
|
||||
.accel_tap = {
|
||||
.axis = axis,
|
||||
.direction = direction,
|
||||
},
|
||||
};
|
||||
|
||||
event_put(&e);
|
||||
}
|
||||
|
||||
static void lis3dh_IRQ2_handler(bool *should_context_switch) {
|
||||
// vibe sometimes triggers the tap interrupt.
|
||||
// if vibe is on, we disregard the interrupt
|
||||
if (vibes_get_vibe_strength() == VIBE_STRENGTH_OFF) {
|
||||
PebbleEvent e = {
|
||||
.type = PEBBLE_CALLBACK_EVENT,
|
||||
.callback = {
|
||||
.callback = prv_handle_tap,
|
||||
.data = NULL
|
||||
}
|
||||
};
|
||||
|
||||
*should_context_switch = event_put_isr(&e);
|
||||
}
|
||||
}
|
||||
|
||||
void accel_set_running(bool running) {
|
||||
s_running = running;
|
||||
}
|
||||
|
||||
void accel_set_num_samples(uint8_t num_samples) {
|
||||
if (num_samples == 0) {
|
||||
// peek mode, no FIFO
|
||||
lis3dh_set_fifo_mode(MODE_BYPASS);
|
||||
lis3dh_disable_fifo();
|
||||
} else {
|
||||
lis3dh_set_fifo_wtm(num_samples - 1);
|
||||
// clear fifo
|
||||
lis3dh_set_fifo_mode(MODE_BYPASS);
|
||||
// wait 1 ms
|
||||
psleep(10);
|
||||
lis3dh_set_fifo_mode(MODE_STREAM);
|
||||
lis3dh_enable_fifo();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static void prv_read_samples(void *data) {
|
||||
uint8_t src_reg;
|
||||
lis3dh_read(LIS3DH_FIFO_SRC_REG, 1, &src_reg);
|
||||
uint8_t num_samples = src_reg & FSS_MASK;
|
||||
|
||||
AccelRawData accel_raw_data[num_samples];
|
||||
if (src_reg & FIFO_OVRN) {
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Fifo overrun");
|
||||
analytics_inc(ANALYTICS_DEVICE_METRIC_ACCEL_FIFO_OVERRUN_COUNT, AnalyticsClient_System);
|
||||
}
|
||||
|
||||
if (src_reg & FIFO_WTM) {
|
||||
accel_get_data(accel_raw_data, num_samples);
|
||||
if (num_samples > 0) {
|
||||
s_latest_reading = accel_raw_data[num_samples-1];
|
||||
}
|
||||
lis3dh_lock();
|
||||
if (s_buffer.clients) {
|
||||
// Only buffer the data if we have clients that are subscribed.
|
||||
if (!shared_circular_buffer_write(&s_buffer, (uint8_t *)accel_raw_data, num_samples * sizeof(AccelRawData),
|
||||
false /*advance_slackers*/)) {
|
||||
// Buffer is full, one or more clients will get dropped data
|
||||
|
||||
// We have one or more clients who fell behind reading out of the buffer. Try again, but this time
|
||||
// resetting the slowest clients until there is room.
|
||||
PBL_ASSERTN(shared_circular_buffer_write(&s_buffer, (uint8_t *)accel_raw_data, num_samples * sizeof(AccelRawData),
|
||||
true /*advance_slackers*/));
|
||||
}
|
||||
}
|
||||
lis3dh_unlock();
|
||||
}
|
||||
|
||||
// Record timestamp of newest data in the queue
|
||||
time_t time_s;
|
||||
uint16_t time_ms;
|
||||
rtc_get_time_ms(&time_s, &time_ms);
|
||||
s_latest_timestamp = ((uint64_t)time_s) * 1000 + time_ms;
|
||||
|
||||
if (num_samples == 0) {
|
||||
accel_reset_pending_accel_event();
|
||||
return;
|
||||
}
|
||||
|
||||
accel_manager_dispatch_data();
|
||||
}
|
||||
|
||||
uint64_t accel_get_latest_timestamp(void) {
|
||||
return s_latest_timestamp;
|
||||
}
|
||||
|
||||
static void lis3dh_IRQ1_handler(bool *should_context_switch) {
|
||||
// It's possible that this interrupt could be leftover after turning accel off.
|
||||
if (!s_running) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Only post a new event if the prior one has been picked up. This prevents us from flooding the KernelMain
|
||||
// queue
|
||||
if (!s_pending_accel_event) {
|
||||
s_pending_accel_event = true;
|
||||
PebbleEvent e = {
|
||||
.type = PEBBLE_CALLBACK_EVENT,
|
||||
.callback = {
|
||||
.callback = prv_read_samples,
|
||||
.data = NULL
|
||||
}
|
||||
};
|
||||
*should_context_switch = event_put_isr(&e);
|
||||
}
|
||||
}
|
||||
|
||||
//! Returns the latest accel reading
|
||||
void accel_get_latest_reading(AccelRawData *data) {
|
||||
*data = s_latest_reading;
|
||||
}
|
||||
|
||||
//! Clears the pending accel event boolean. Called by KernelMain once it receives the accel_manager_dispatch_data
|
||||
//! callback
|
||||
void accel_reset_pending_accel_event(void) {
|
||||
s_pending_accel_event = false;
|
||||
}
|
||||
|
||||
//! Adds a consumer to the circular buffer
|
||||
//! @client which client to add
|
||||
void accel_add_consumer(SharedCircularBufferClient *client) {
|
||||
lis3dh_lock();
|
||||
PBL_ASSERTN(shared_circular_buffer_add_client(&s_buffer, client));
|
||||
lis3dh_unlock();
|
||||
}
|
||||
|
||||
|
||||
//! Removes a consumer from the circular buffer
|
||||
//! @client which client to remove
|
||||
void accel_remove_consumer(SharedCircularBufferClient *client) {
|
||||
lis3dh_lock();
|
||||
shared_circular_buffer_remove_client(&s_buffer, client);
|
||||
lis3dh_unlock();
|
||||
}
|
||||
|
||||
|
||||
//! Returns number of samples actually read.
|
||||
//! @param data The buffer to read the data into
|
||||
//! @client which client is reading
|
||||
//! @param max_samples Size of buffer in samples
|
||||
//! @param subsample_num Subsampling numerator
|
||||
//! @param subsample_den Subsampling denominator
|
||||
//! @return The actual number of samples read
|
||||
uint32_t accel_consume_data(AccelRawData *data, SharedCircularBufferClient *client, uint32_t max_samples,
|
||||
uint16_t subsample_num, uint16_t subsample_den) {
|
||||
uint16_t items_read;
|
||||
PBL_ASSERTN(accel_running());
|
||||
lis3dh_lock();
|
||||
{
|
||||
shared_circular_buffer_subsample_items(&s_buffer, client, sizeof(AccelRawData), max_samples, subsample_num,
|
||||
subsample_den, (uint8_t *)data, &items_read);
|
||||
}
|
||||
lis3dh_unlock();
|
||||
ACCEL_LOG_DEBUG("%"PRIu16" samples (from %"PRIu32" requested) were read for %p",
|
||||
items_read, max_samples, client);
|
||||
return (items_read);
|
||||
}
|
||||
|
||||
|
||||
int accel_peek(AccelData* data) {
|
||||
if (!s_running) {
|
||||
return (-1);
|
||||
}
|
||||
|
||||
// No peeking if we're in FIFO mode.
|
||||
if (lis3dh_get_fifo_mode() == MODE_STREAM) {
|
||||
return (-2);
|
||||
}
|
||||
|
||||
accel_get_data((AccelRawData*)data, 1);
|
||||
|
||||
return (0);
|
||||
}
|
||||
|
||||
|
||||
// Compute and return the device's delta position to help determine movement as idle.
|
||||
static uint32_t prv_compute_delta_pos(AccelRawData *cur_pos, AccelRawData *last_pos) {
|
||||
return abs(last_pos->x - cur_pos->x) + abs(last_pos->y - cur_pos->y) + abs(last_pos->z - cur_pos->z);
|
||||
}
|
||||
|
||||
|
||||
// Return true if we are "idle". We check for no movement for at least the last hour (the analytics snapshot
|
||||
// position is updated once/hour).
|
||||
bool accel_is_idle(void) {
|
||||
if (!s_is_idle) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// It was idle recently, see if it's still idle. Note we are avoiding reading the accel hardwware again here
|
||||
// to keep this call as lightweight as possible. Instead we are just comparing the last read value with
|
||||
// the value last captured by analytics (which does so on an hourly heartbeat).
|
||||
AccelRawData accel_data;
|
||||
accel_get_last_data((AccelRawData*)&accel_data);
|
||||
s_is_idle = (prv_compute_delta_pos(&accel_data, &s_last_analytics_position) < ACCEL_MAX_IDLE_DELTA);
|
||||
return s_is_idle;
|
||||
}
|
||||
|
||||
|
||||
static bool prv_get_accel_data(AccelRawData *accel_data) {
|
||||
bool running = accel_running();
|
||||
if (!running) {
|
||||
if (!accel_start()) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
if (lis3dh_get_fifo_mode() != MODE_STREAM) {
|
||||
accel_get_data((AccelRawData*)accel_data, 1);
|
||||
} else {
|
||||
accel_get_last_data((AccelRawData*)accel_data);
|
||||
}
|
||||
if (!running) {
|
||||
accel_stop();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// Analytics Metrics
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
void analytics_external_collect_accel_xyz_delta(void) {
|
||||
AccelRawData accel_data;
|
||||
if (prv_get_accel_data(&accel_data)) {
|
||||
uint32_t delta = prv_compute_delta_pos(&accel_data, &s_last_analytics_position);
|
||||
s_is_idle = (delta < ACCEL_MAX_IDLE_DELTA);
|
||||
s_last_analytics_position = accel_data;
|
||||
analytics_set(ANALYTICS_DEVICE_METRIC_ACCEL_XYZ_DELTA, delta, AnalyticsClient_System);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Self Test
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
|
||||
bool accel_self_test(void) {
|
||||
AccelRawData data;
|
||||
AccelRawData data_st;
|
||||
|
||||
if (!accel_start()) {
|
||||
PBL_LOG(LOG_LEVEL_ERROR, "Self test failed, could not start accel");
|
||||
return false;
|
||||
}
|
||||
|
||||
psleep(10);
|
||||
|
||||
accel_get_data(&data, 1);
|
||||
|
||||
lis3dh_enter_self_test_mode(SELF_TEST_MODE_ONE);
|
||||
// ST recommends sleeping for 1ms after programming the module to
|
||||
// enter self-test mode; a 100x factor of safety ought to be
|
||||
// sufficient
|
||||
psleep(100);
|
||||
|
||||
accel_get_data(&data_st, 1);
|
||||
|
||||
lis3dh_exit_self_test_mode();
|
||||
accel_stop();
|
||||
|
||||
// [MJZ] I have no idea how to interpret the data coming out of the
|
||||
// accel's self-test mode. If I could make sense of the
|
||||
// incomprehensible datasheet, I would be able to check if the accel
|
||||
// output matches the expected values
|
||||
return ABS(data_st.x) > ABS(data.x);
|
||||
}
|
||||
|
||||
void accel_set_shake_sensitivity_high(bool sensitivity_high) {
|
||||
// Configure the threshold level at which the LIS3DH will think motion has occurred
|
||||
if (sensitivity_high) {
|
||||
lis3dh_set_interrupt_threshold(
|
||||
BOARD_CONFIG_ACCEL.accel_config.shake_thresholds[AccelThresholdLow]);
|
||||
} else {
|
||||
lis3dh_set_interrupt_threshold(
|
||||
BOARD_CONFIG_ACCEL.accel_config.shake_thresholds[AccelThresholdHigh]);
|
||||
}
|
||||
}
|
118
src/fw/drivers/imu/lis3dh/lis3dh.h
Normal file
118
src/fw/drivers/imu/lis3dh/lis3dh.h
Normal file
|
@ -0,0 +1,118 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "applib/accel_service.h"
|
||||
|
||||
static const int16_t LIS3DH_COUNTS_PER_G = 4096;
|
||||
static const int16_t LIS3DH_SAMPLING_RATE_HZ = 50;
|
||||
|
||||
static const int LIS3DH_MIN_VALUE = -32768;
|
||||
static const int LIS3DH_MAX_VALUE = 32767;
|
||||
static const uint8_t LIS3DH_WHOAMI_BYTE = 0x33;
|
||||
|
||||
// Computing AccelSamplingRate * LIS3DH_TIME_LIMIT_MULT / LIS3DH_TIME_LIMIT_DIV
|
||||
// yields the correct setting for the TIME_LIMIT register
|
||||
static const int LIS3DH_TIME_LIMIT_MULT = 2240;
|
||||
static const int LIS3DH_TIME_LIMIT_DIV = 1000;
|
||||
// Computing AccelSamplingRate * LIS3DH_TIME_LATENCY_MULT / LIS3DH_TIME_LATENCY_DIV
|
||||
// yields the correct setting for the TIME_LIMIT register
|
||||
static const int LIS3DH_TIME_LATENCY_MULT = 1280;
|
||||
static const int LIS3DH_TIME_LATENCY_DIV = 1000;
|
||||
// Computing AccelSamplingRate * LIS3DH_TIME_WINDOW_MULT / LIS3DH_TIME_WINDOW_DIV
|
||||
// yields the correct setting for the TIME_WINDOW register
|
||||
static const int LIS3DH_TIME_WINDOW_MULT = 5120;
|
||||
static const int LIS3DH_TIME_WINDOW_DIV = 1000;
|
||||
// Computing AccelScale * LIS3DH_THRESHOLD_MULT / LIS3DH_THRESHOLD_DIV
|
||||
// yields the correct setting for the CLICK_THS register
|
||||
static const int LIS3DH_THRESHOLD_MULT = 24;
|
||||
static const int LIS3DH_THRESHOLD_DIV = 1;
|
||||
|
||||
typedef enum {
|
||||
SELF_TEST_MODE_OFF,
|
||||
SELF_TEST_MODE_ONE,
|
||||
SELF_TEST_MODE_TWO,
|
||||
SELF_TEST_MODE_COUNT
|
||||
} SelfTestMode;
|
||||
|
||||
//! Valid accelerometer scales, in g's
|
||||
typedef enum {
|
||||
LIS3DH_SCALE_UNKNOWN = 0,
|
||||
LIS3DH_SCALE_16G = 16,
|
||||
LIS3DH_SCALE_8G = 8,
|
||||
LIS3DH_SCALE_4G = 4,
|
||||
LIS3DH_SCALE_2G = 2,
|
||||
} Lis3dhScale;
|
||||
|
||||
void lis3dh_init(void);
|
||||
|
||||
void lis3dh_lock(void);
|
||||
void lis3dh_unlock(void);
|
||||
void lis3dh_init_mutex(void);
|
||||
|
||||
void enable_lis3dh_interrupts(void);
|
||||
void disable_lis3dh_interrupts(void);
|
||||
|
||||
bool lis3dh_sanity_check(void);
|
||||
|
||||
// Poke specific registers
|
||||
void lis3dh_disable_click(void);
|
||||
void lis3dh_enable_click(void);
|
||||
|
||||
void lis3dh_disable_fifo(void);
|
||||
void lis3dh_enable_fifo(void);
|
||||
bool lis3dh_is_fifo_enabled(void);
|
||||
|
||||
void lis3dh_power_up(void);
|
||||
void lis3dh_power_down(void);
|
||||
|
||||
void lis3dh_set_interrupt_axis(AccelAxisType axis, bool double_click);
|
||||
|
||||
uint8_t lis3dh_get_interrupt_threshold();
|
||||
static const uint8_t LIS3DH_MAX_THRESHOLD = 0x7f;
|
||||
void lis3dh_set_interrupt_threshold(uint8_t threshold);
|
||||
|
||||
uint8_t lis3dh_get_interrupt_time_limit();
|
||||
static const uint8_t LIS3DH_MAX_TIME_LIMIT = 0x7f;
|
||||
void lis3dh_set_interrupt_time_limit(uint8_t time_limit);
|
||||
|
||||
uint8_t lis3dh_get_click_latency();
|
||||
static const uint8_t LIS3DH_MAX_CLICK_LATENCY = 0xff;
|
||||
void lis3dh_set_click_latency(uint8_t latency);
|
||||
|
||||
uint8_t lis3dh_get_click_window();
|
||||
static const uint8_t LIS3DH_MAX_CLICK_WINDOW = 0xff;
|
||||
void lis3dh_set_click_window(uint8_t window);
|
||||
|
||||
bool lis3dh_set_fifo_mode(uint8_t);
|
||||
uint8_t lis3dh_get_fifo_mode(void);
|
||||
|
||||
bool lis3dh_set_fifo_wtm(uint8_t);
|
||||
uint8_t lis3dh_get_fifo_wtm(void);
|
||||
|
||||
|
||||
bool lis3dh_enter_self_test_mode(SelfTestMode mode);
|
||||
void lis3dh_exit_self_test_mode(void);
|
||||
bool lis3dh_self_test(void);
|
||||
|
||||
bool lis3dh_config_set_defaults();
|
||||
|
||||
bool lis3dh_read(uint8_t address, uint8_t read_size, uint8_t *buffer);
|
||||
bool lis3dh_write(uint8_t address, uint8_t write_size, const uint8_t *buffer);
|
126
src/fw/drivers/imu/lis3dh/registers.h
Normal file
126
src/fw/drivers/imu/lis3dh/registers.h
Normal file
|
@ -0,0 +1,126 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
// 0x00 - 0x06: reserved
|
||||
static const uint8_t LIS3DH_STATUS_REG_AUX = 0x07;
|
||||
static const uint8_t LIS3DH_OUT_ADC1_L = 0x08;
|
||||
static const uint8_t LIS3DH_OUT_ADC1_H = 0x09;
|
||||
static const uint8_t LIS3DH_OUT_ADC2_L = 0x0a;
|
||||
static const uint8_t LIS3DH_OUT_ADC2_H = 0x0b;
|
||||
static const uint8_t LIS3DH_OUT_ADC3_L = 0x0c;
|
||||
static const uint8_t LIS3DH_OUT_ADC3_H = 0x0d;
|
||||
static const uint8_t LIS3DH_INT_COUNTER_REG = 0x0e;
|
||||
static const uint8_t LIS3DH_WHO_AM_I = 0x0f;
|
||||
// 0x10 - 0x1E: reserved
|
||||
static const uint8_t LIS3DH_TEMP_CFG_REG = 0x1f;
|
||||
static const uint8_t LIS3DH_CTRL_REG1 = 0x20;
|
||||
static const uint8_t LIS3DH_CTRL_REG2 = 0x21;
|
||||
static const uint8_t LIS3DH_CTRL_REG3 = 0x22;
|
||||
static const uint8_t LIS3DH_CTRL_REG4 = 0x23;
|
||||
static const uint8_t LIS3DH_CTRL_REG5 = 0x24;
|
||||
static const uint8_t LIS3DH_CTRL_REG6 = 0x25;
|
||||
static const uint8_t LIS3DH_REFERENCE = 0x26;
|
||||
static const uint8_t LIS3DH_STATUS_REG2 = 0x27;
|
||||
static const uint8_t LIS3DH_OUT_X_L = 0x28;
|
||||
static const uint8_t LIS3DH_OUT_X_H = 0x29;
|
||||
static const uint8_t LIS3DH_OUT_Y_L = 0x2a;
|
||||
static const uint8_t LIS3DH_OUT_Y_H = 0x2b;
|
||||
static const uint8_t LIS3DH_OUT_Z_L = 0x2c;
|
||||
static const uint8_t LIS3DH_OUT_Z_H = 0x2d;
|
||||
static const uint8_t LIS3DH_FIFO_CTRL_REG = 0x2e;
|
||||
static const uint8_t LIS3DH_FIFO_SRC_REG = 0x2f;
|
||||
static const uint8_t LIS3DH_INT1_CFG = 0x30;
|
||||
static const uint8_t LIS3DH_INT1_SRC = 0x31;
|
||||
static const uint8_t LIS3DH_INT1_THS = 0x32;
|
||||
static const uint8_t LIS3DH_INT1_DURATION = 0x33;
|
||||
// 0x34 - 0x37: reserved
|
||||
static const uint8_t LIS3DH_CLICK_CFG = 0x38;
|
||||
static const uint8_t LIS3DH_CLICK_SRC = 0x39;
|
||||
static const uint8_t LIS3DH_CLICK_THS = 0x3a;
|
||||
static const uint8_t LIS3DH_TIME_LIMIT = 0x3b;
|
||||
static const uint8_t LIS3DH_TIME_LATENCY = 0x3c;
|
||||
static const uint8_t LIS3DH_TIME_WINDOW = 0x3d;
|
||||
static const uint8_t LIS3DH_ACT_THS = 0x3e;
|
||||
static const uint8_t LIS3DH_INACT_DUR = 0x3f;
|
||||
|
||||
// CTRL_REG1
|
||||
static const uint8_t ODR3 = (1 << 7);
|
||||
static const uint8_t ODR2 = (1 << 6);
|
||||
static const uint8_t ODR1 = (1 << 5);
|
||||
static const uint8_t ODR0 = (1 << 4);
|
||||
static const uint8_t ODR_MASK = (0xf0);
|
||||
static const uint8_t LPen = (1 << 3);
|
||||
static const uint8_t Zen = (1 << 2);
|
||||
static const uint8_t Yen = (1 << 1);
|
||||
static const uint8_t Xen = (1 << 0);
|
||||
|
||||
// CTRL_REG3
|
||||
static const uint8_t I1_CLICK = (1 << 7);
|
||||
static const uint8_t I1_AOI1 = (1 << 6);
|
||||
static const uint8_t I1_DTRDY = (1 << 4);
|
||||
static const uint8_t I1_WTM = (1 << 2);
|
||||
static const uint8_t I1_OVRN = (1 << 1);
|
||||
|
||||
//CTRL_REG4
|
||||
static const uint8_t BDU = (1 << 7);
|
||||
static const uint8_t BLE = (1 << 6);
|
||||
static const uint8_t FS1 = (1 << 5);
|
||||
static const uint8_t FS0 = (1 << 4);
|
||||
static const uint8_t HR = (1 << 3);
|
||||
static const uint8_t ST1 = (1 << 2);
|
||||
static const uint8_t ST0 = (1 << 1);
|
||||
static const uint8_t SIM = (1 << 0);
|
||||
static const uint8_t FS_MASK = 0x30;
|
||||
|
||||
//CTRL_REG5
|
||||
static const uint8_t FIFO_EN = (1 << 6);
|
||||
|
||||
//CTRL_REG6
|
||||
static const uint8_t I2_CLICK = (1 << 7);
|
||||
|
||||
// CLICK_CFG
|
||||
static const uint8_t ZD = (1 << 5);
|
||||
static const uint8_t ZS = (1 << 4);
|
||||
static const uint8_t YD = (1 << 3);
|
||||
static const uint8_t YS = (1 << 2);
|
||||
static const uint8_t XD = (1 << 1);
|
||||
static const uint8_t XS = (1 << 0);
|
||||
|
||||
// CLICK_SRC
|
||||
static const uint8_t IA = 1 << 6;
|
||||
static const uint8_t DCLICK = 1 << 5;
|
||||
static const uint8_t SCLICK = 1 << 4;
|
||||
static const uint8_t Sign = 1 << 3;
|
||||
static const uint8_t ZClick = 1 << 2;
|
||||
static const uint8_t YClick = 1 << 1;
|
||||
static const uint8_t XClick = 1 << 0;
|
||||
|
||||
// FIFO_CTRL_REG
|
||||
static const uint8_t MODE_BYPASS = 0x0;
|
||||
static const uint8_t MODE_FIFO = (0x1 << 6);
|
||||
static const uint8_t MODE_STREAM = (0x1 << 7);
|
||||
static const uint8_t MODE_MASK = 0xc0;
|
||||
static const uint8_t THR_MASK = 0x1f;
|
||||
|
||||
// FIFO_SRC_REG
|
||||
static const uint8_t FIFO_WTM = (0x1 << 7);
|
||||
static const uint8_t FIFO_OVRN = (0x1 << 6);
|
||||
static const uint8_t FIFO_EMPTY = (0x1 << 5);
|
||||
static const uint8_t FSS_MASK = 0x1f;
|
269
src/fw/drivers/imu/mag3110/mag3110.c
Normal file
269
src/fw/drivers/imu/mag3110/mag3110.c
Normal file
|
@ -0,0 +1,269 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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 "mag3110.h"
|
||||
|
||||
#include "board/board.h"
|
||||
#include "console/prompt.h"
|
||||
#include "drivers/exti.h"
|
||||
#include "drivers/gpio.h"
|
||||
#include "drivers/i2c.h"
|
||||
#include "drivers/mag.h"
|
||||
#include "drivers/periph_config.h"
|
||||
#include "kernel/events.h"
|
||||
#include "system/logging.h"
|
||||
#include "os/mutex.h"
|
||||
#include "system/passert.h"
|
||||
#include "kernel/util/sleep.h"
|
||||
|
||||
#define STM32F2_COMPATIBLE
|
||||
#define STM32F4_COMPATIBLE
|
||||
#define STM32F7_COMPATIBLE
|
||||
#include <mcu.h>
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <stdint.h>
|
||||
|
||||
static PebbleMutex *s_mag_mutex;
|
||||
|
||||
static bool s_initialized = false;
|
||||
|
||||
static int s_use_refcount = 0;
|
||||
|
||||
// MAG3110 Register Address Map
|
||||
#define DR_STATUS_REG 0x00
|
||||
#define OUT_X_MSB_REG 0x01 // a 6-byte read here will return X, Y, Z data
|
||||
#define WHO_AM_I_REG 0x07
|
||||
#define SYSMOD_REG 0x08
|
||||
#define CTRL_REG1 0x10
|
||||
#define CTRL_REG2 0x11
|
||||
|
||||
static bool mag3110_read(uint8_t reg_addr, uint8_t data_len, uint8_t *data) {
|
||||
return i2c_read_register_block(I2C_MAG3110, reg_addr, data_len, data);
|
||||
}
|
||||
|
||||
static bool mag3110_write(uint8_t reg_addr, uint8_t data) {
|
||||
return i2c_write_register_block(I2C_MAG3110, reg_addr, 1, &data);
|
||||
}
|
||||
|
||||
static void mag3110_interrupt_handler(bool *should_context_switch) {
|
||||
if (s_use_refcount == 0) {
|
||||
// Spurious interrupt firing after we've already turned off the mag. Just ignore.
|
||||
return;
|
||||
}
|
||||
|
||||
// TODO: May want to use a timer, lowers worst case latency
|
||||
PebbleEvent e = {
|
||||
.type = PEBBLE_ECOMPASS_SERVICE_EVENT,
|
||||
};
|
||||
|
||||
*should_context_switch = event_put_isr(&e);
|
||||
}
|
||||
|
||||
//! Move the mag into standby mode, which is a low power mode where we're not actively sampling
|
||||
//! the sensor or firing interrupts.
|
||||
static bool prv_enter_standby_mode(void) {
|
||||
// Ask to enter standby mode
|
||||
if (!mag3110_write(CTRL_REG1, 0x00)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Wait for the sysmod register to read that we're now in standby mode. This can take up to
|
||||
// 1/ODR to respond. Since we only support speeds as slow as 5Hz, that means we may be waiting
|
||||
// for up to 200ms for this part to become ready.
|
||||
const int NUM_ATTEMPTS = 300; // 200ms + some padding for safety
|
||||
for (int i = 0; i < NUM_ATTEMPTS; ++i) {
|
||||
uint8_t sysmod = 0;
|
||||
if (!mag3110_read(SYSMOD_REG, 1, &sysmod)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (sysmod == 0) {
|
||||
// We're done and we're now in standby!
|
||||
return true;
|
||||
}
|
||||
|
||||
// Wait at least 1ms before asking again
|
||||
psleep(2);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// Ask the compass for a 8-bit value that's programmed into the IC at the
|
||||
// factory. Useful as a sanity check to make sure everything came up properly.
|
||||
bool mag3110_check_whoami(void) {
|
||||
static const uint8_t COMPASS_WHOAMI_BYTE = 0xc4;
|
||||
|
||||
uint8_t whoami = 0;
|
||||
|
||||
mag_use();
|
||||
mag3110_read(WHO_AM_I_REG, 1, &whoami);
|
||||
mag_release();
|
||||
|
||||
PBL_LOG(LOG_LEVEL_DEBUG, "Read compass whoami byte 0x%x, expecting 0x%x",
|
||||
whoami, COMPASS_WHOAMI_BYTE);
|
||||
|
||||
return (whoami == COMPASS_WHOAMI_BYTE);
|
||||
}
|
||||
|
||||
void mag3110_init(void) {
|
||||
if (s_initialized) {
|
||||
return;
|
||||
}
|
||||
s_mag_mutex = mutex_create();
|
||||
|
||||
s_initialized = true;
|
||||
|
||||
if (!mag3110_check_whoami()) {
|
||||
PBL_LOG(LOG_LEVEL_WARNING, "Failed to query Mag");
|
||||
}
|
||||
gpio_input_init(&BOARD_CONFIG_MAG.mag_int_gpio);
|
||||
|
||||
exti_configure_pin(BOARD_CONFIG_MAG.mag_int, ExtiTrigger_Rising, mag3110_interrupt_handler);
|
||||
}
|
||||
|
||||
void mag_use(void) {
|
||||
PBL_ASSERTN(s_initialized);
|
||||
|
||||
mutex_lock(s_mag_mutex);
|
||||
|
||||
if (s_use_refcount == 0) {
|
||||
i2c_use(I2C_MAG3110);
|
||||
exti_enable(BOARD_CONFIG_MAG.mag_int);
|
||||
}
|
||||
++s_use_refcount;
|
||||
|
||||
mutex_unlock(s_mag_mutex);
|
||||
}
|
||||
|
||||
void mag_release(void) {
|
||||
PBL_ASSERTN(s_initialized && s_use_refcount != 0);
|
||||
|
||||
mutex_lock(s_mag_mutex);
|
||||
|
||||
--s_use_refcount;
|
||||
if (s_use_refcount == 0) {
|
||||
// We need to put the magnetometer into standby mode and read the data register to reset
|
||||
// the state so it's ready for next time.
|
||||
prv_enter_standby_mode();
|
||||
|
||||
uint8_t raw_data[7];
|
||||
// DR_STATUS_REG is immediately before data registers
|
||||
mag3110_read(DR_STATUS_REG, sizeof(raw_data), raw_data);
|
||||
|
||||
// Now we can actually remove power and disable the interrupt
|
||||
i2c_release(I2C_MAG3110);
|
||||
exti_disable(BOARD_CONFIG_MAG.mag_int);
|
||||
}
|
||||
|
||||
mutex_unlock(s_mag_mutex);
|
||||
}
|
||||
|
||||
// aligns magnetometer data with the coordinate system we have adopted
|
||||
// for the watch
|
||||
static int16_t align_coord_system(int axis, uint8_t *raw_data) {
|
||||
int offset = 2 * BOARD_CONFIG_MAG.mag_config.axes_offsets[axis];
|
||||
bool do_invert = BOARD_CONFIG_MAG.mag_config.axes_inverts[axis];
|
||||
int16_t mag_field_strength = ((raw_data[offset] << 8) | raw_data[offset + 1]);
|
||||
mag_field_strength *= (do_invert ? -1 : 1);
|
||||
return (mag_field_strength);
|
||||
}
|
||||
|
||||
// callers responsibility to know if there is valid data to be read
|
||||
MagReadStatus mag_read_data(MagData *data) {
|
||||
mutex_lock(s_mag_mutex);
|
||||
|
||||
if (s_use_refcount == 0) {
|
||||
mutex_unlock(s_mag_mutex);
|
||||
return (MagReadMagOff);
|
||||
}
|
||||
|
||||
MagReadStatus rv = MagReadSuccess;
|
||||
uint8_t raw_data[7];
|
||||
|
||||
// DR_STATUS_REG is immediately before data registers
|
||||
if (!mag3110_read(DR_STATUS_REG, sizeof(raw_data), raw_data)) {
|
||||
rv = MagReadCommunicationFail;
|
||||
goto done;
|
||||
}
|
||||
|
||||
// TODO: shouldn't happen at low sample rate, but handle case where some data
|
||||
// is overwritten
|
||||
if ((raw_data[0] & 0xf0) != 0) {
|
||||
PBL_LOG(LOG_LEVEL_INFO, "Some Mag Sample Data was overwritten, "
|
||||
"dr_status=0x%x", raw_data[0]);
|
||||
rv = MagReadClobbered; // we still need to read the data to clear the int
|
||||
}
|
||||
|
||||
// map raw data to watch coord system
|
||||
data->x = align_coord_system(0, &raw_data[1]);
|
||||
data->y = align_coord_system(1, &raw_data[1]);
|
||||
data->z = align_coord_system(2, &raw_data[1]);
|
||||
|
||||
done:
|
||||
mutex_unlock(s_mag_mutex);
|
||||
return (rv);
|
||||
}
|
||||
|
||||
bool mag_change_sample_rate(MagSampleRate rate) {
|
||||
mutex_lock(s_mag_mutex);
|
||||
|
||||
if (s_use_refcount == 0) {
|
||||
mutex_unlock(s_mag_mutex);
|
||||
return (true);
|
||||
}
|
||||
|
||||
bool success = false;
|
||||
|
||||
// Enter standby state since we can only change sample rate in this mode.
|
||||
if (!prv_enter_standby_mode()) {
|
||||
goto done;
|
||||
}
|
||||
|
||||
// See Table 25 in the data sheet for these values for the CTRL_REG1 register. We leave the
|
||||
// oversampling values at zero and just set the data rate bits.
|
||||
uint8_t new_sample_rate_value = 0;
|
||||
switch(rate) {
|
||||
case MagSampleRate20Hz:
|
||||
new_sample_rate_value = 0x1 << 6;
|
||||
break;
|
||||
case MagSampleRate5Hz:
|
||||
new_sample_rate_value = 0x2 << 6;
|
||||
break;
|
||||
}
|
||||
|
||||
// Write the new sample rate as well as set the bottom bit of the ctrl register to put us into
|
||||
// active mode.
|
||||
if (!mag3110_write(CTRL_REG1, new_sample_rate_value | 0x01)) {
|
||||
goto done;
|
||||
}
|
||||
|
||||
success = true;
|
||||
done:
|
||||
mutex_unlock(s_mag_mutex);
|
||||
|
||||
return (success);
|
||||
}
|
||||
|
||||
void mag_start_sampling(void) {
|
||||
mag_use();
|
||||
|
||||
// enable automatic magnetic sensor reset & RAW mode
|
||||
mag3110_write(CTRL_REG2, 0xA0);
|
||||
|
||||
mag_change_sample_rate(MagSampleRate5Hz);
|
||||
}
|
20
src/fw/drivers/imu/mag3110/mag3110.h
Normal file
20
src/fw/drivers/imu/mag3110/mag3110.h
Normal file
|
@ -0,0 +1,20 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
void mag3110_init(void);
|
||||
|
40
src/fw/drivers/imu/mag_null.c
Normal file
40
src/fw/drivers/imu/mag_null.c
Normal file
|
@ -0,0 +1,40 @@
|
|||
/*
|
||||
* Copyright 2024 Google LLC
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
// Null Magnetometer Driver
|
||||
|
||||
#if CAPABILITY_HAS_MAGNETOMETER
|
||||
#error This driver is only intended for boards without a magnetometer
|
||||
#endif
|
||||
|
||||
#include "drivers/mag.h"
|
||||
|
||||
void mag_use(void) {
|
||||
}
|
||||
|
||||
void mag_start_sampling(void) {
|
||||
}
|
||||
|
||||
void mag_release(void) {
|
||||
}
|
||||
|
||||
MagReadStatus mag_read_data(MagData *data) {
|
||||
return MagReadNoMag;
|
||||
}
|
||||
|
||||
bool mag_change_sample_rate(MagSampleRate rate) {
|
||||
return false;
|
||||
}
|
Loading…
Add table
Add a link
Reference in a new issue