Mirrors/pebble
1
0
Fork 0
mirror of https://github.com/google/pebble.git synced 2025-05-28 05:53:12 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions
pebble/src/fw/drivers/imu/bma255/bma255.c
Josh Soref ee41f88dad spelling: integer 
Signed-off-by: Josh Soref <2119212+jsoref@users.noreply.github.com>
2025-01-29 00:03:25 -05:00

898 lines
31 KiB
C
Raw Blame History

/*
* 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 integer 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();
}
Reference in a new issue View git blame Copy permalink