Mirrors/pebble
1
0
Fork 0
mirror of https://github.com/google/pebble.git synced 2025-06-06 10:13:12 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

Import of the watch repository from Pebble

Browse source
This commit is contained in:
Matthieu Jeanson 2024-12-12 16:43:03 -08:00 • committed by Katharine Berry
commit 3b92768480
10334 changed files with 2564465 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

898
src/fw/drivers/imu/bma255/bma255.c Normal file
View 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
View 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
View 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
View 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
View 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
View file

File diff suppressed because it is too large Load diff

36
src/fw/drivers/imu/bmi160/bmi160.h Normal file
View 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
View 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
View 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
View 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 = &reg, .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 = &reg, .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 = &reg, .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
View 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
View 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
View 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
View 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
View 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
View 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, &reg);
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
View 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
View 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
View 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
View 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
View 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
View 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;
}