jh.chun/VesiScan-Basic_imu
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
jhchun
2026-04-24 10:52:47 +09:00
commit 1707dc6349
1423 changed files with 1161776 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
project/ble_peripheral/ble_app_bladder_patch/measurement/adc121s051/dr_adc121s051.c
+1426
View File
File diff suppressed because it is too large Load Diff
project/ble_peripheral/ble_app_bladder_patch/measurement/adc121s051/dr_adc121s051.h
+502
View File
@@ -0,0 +1,502 @@
/*******************************************************************************
* @file dr_adc121s051.h
* @brief ADC121S051 12-bit ADC Driver for nRF52840
* For 1.2MHz Piezo Echo Envelope Detection
* @author Charles KWON
* @date 2025-12-15
*
* @details This driver reads the envelope-detected DC level from piezo echo.
*
* Signal Flow:
*
* [Piezo TX] [Echo RX] [Envelope] [ADC] [MCU]
* 1.2MHz --> Reflect --> Detector --> DC Level --> Digital
* burst signal (hardware) reading value
*
* The envelope detector circuit converts the 1.2MHz echo burst
* into a DC voltage proportional to the echo amplitude.
* ADC samples this DC level for amplitude measurement.
*
* @note Hardware: Texas Instruments ADC121S051
* - 12-bit resolution (0-4095)
* - Sample rate: 200-500 ksps
* - Input range: 0V to VA
* - SPI interface (software bit-bang)
******************************************************************************/
#ifndef DR_ADC121S051_H
#define DR_ADC121S051_H
#include <stdint.h>
#include <stdbool.h>
#include "nrf_gpio.h"
/*==============================================================================
* PIN CONFIGURATION
*
* WARNING: Never hardcode pin numbers!
* Hardcoding may save a developer's time momentarily,
* but it will also shorten their lifespan.
*============================================================================*/
#define DR_ADC_PIN_SCLK NRF_GPIO_PIN_MAP(0, 14) /**< Serial Clock */
#define DR_ADC_PIN_SDATA NRF_GPIO_PIN_MAP(0, 15) /**< Serial Data (MISO) */
#define DR_ADC_PIN_CS NRF_GPIO_PIN_MAP(0, 19) /**< Chip Select P0.13 -> P0.19 */
/*==============================================================================
* ADC SPECIFICATIONS
*============================================================================*/
#define DR_ADC_RESOLUTION 12 /**< Bits */
#define DR_ADC_MAX_VALUE 4095 /**< 2^12 - 1 */
#define DR_ADC_VREF_MV 3300 /**< Reference voltage (mV) */
/*==============================================================================
* ECHO DETECTION CONFIGURATION
*
* Bladder Measurement Requirements:
* - Target measurement range: 20cm (200mm)
* - SCLK frequency: 8.6MHz (bit-bang SPI)
* - ADC121S051 requires 16 SCLK cycles per sample
* - Actual sample rate: 8.6MHz / 16 = 0.5375MHz = 537.5kHz
* - Actual sample interval: 16 / 8.6MHz = 1.86us
* - Sound speed in tissue: 1540m/s = 1.54mm/us
*
* Formula: samples = distance(mm) * 2 / (1.86us * 1.54mm/us)
* = distance(mm) * 2 / 2.86
* = distance(mm) * 0.7
*
* 10cm = 100mm -> 100 * 0.7 = 70 samples (round-trip 130us)
* 17cm = 170mm -> 170 * 0.7 = 119 samples (round-trip 221us)
* 20cm = 200mm -> 200 * 0.7 = 140 samples (round-trip 260us)
*
* Buffer size: 200 samples * 2 bytes = 400 bytes (RAM 256KB, OK)
* BLE transmission: 140 samples * 2 bytes = 280 bytes (16-bit raw, no packing)
*============================================================================*/
#define DR_ADC_SCLK_MHZ 8.6f /**< SPI bit-bang SCLK frequency */
#define DR_ADC_CLOCKS_PER_SAMPLE 16 /**< ADC121S051: 16 SCLK per sample */
#define DR_ADC_ECHO_SAMPLES_MAX 119 /**< Maximum samples */
#define DR_ADC_ECHO_SAMPLES_DEFAULT 100 /**< Default samples */
#define DR_ADC_SAMPLE_INTERVAL_US 1.86f /**< 16 / 8.6MHz = 1.86us per sample */
#define DR_ADC_SOUND_SPEED_MM_US 1.54f /**< Sound speed in tissue (mm/us) */
/*==============================================================================
* ERROR CODES
*============================================================================*/
typedef enum {
DR_ADC_OK = 0,
DR_ADC_ERR_NOT_INIT,
DR_ADC_ERR_INVALID_PARAM,
DR_ADC_ERR_NO_ECHO
} dr_adc_err_t;
/*==============================================================================
* DATA STRUCTURES
*============================================================================*/
/**
* @brief Single ADC reading result
*/
typedef struct {
uint16_t raw; /**< Raw 12-bit value (0-4095) */
uint32_t voltage_mv; /**< Voltage in millivolts */
} dr_adc_result_t;
/**
* @brief Echo measurement result
*/
typedef struct {
uint16_t peak_raw; /**< Peak amplitude (raw) */
uint32_t peak_mv; /**< Peak amplitude (mV) */
uint16_t peak_index; /**< Sample index of peak */
uint32_t peak_time_us; /**< Time to peak (us) */
uint16_t baseline_raw; /**< Baseline level before echo */
uint16_t num_samples; /**< Number of samples captured */
} dr_adc_echo_t;
/**
* @brief Echo capture configuration
*/
typedef struct {
uint16_t num_samples; /**< Samples to capture (1-200) */
uint16_t threshold_raw; /**< Minimum peak threshold */
uint16_t delay_us; /**< Delay before capture starts */
} dr_adc_echo_config_t;
/*==============================================================================
* INITIALIZATION
*============================================================================*/
/**
* @brief Initialize ADC driver
* @return dr_adc_err_t Error code
*/
dr_adc_err_t dr_adc_init(void);
/**
* @brief Uninitialize ADC driver
*/
void dr_adc_uninit(void);
/**
* @brief Check if initialized
*/
bool dr_adc_is_initialized(void);
/*==============================================================================
* BASIC READ FUNCTIONS
*============================================================================*/
/**
* @brief Read single ADC value
* @param result Pointer to result structure
* @return dr_adc_err_t Error code
*/
dr_adc_err_t dr_adc_read(dr_adc_result_t *result);
/**
* @brief Read raw 12-bit value only
* @param raw_value Pointer to store value
* @return dr_adc_err_t Error code
*/
dr_adc_err_t dr_adc_read_raw(uint16_t *raw_value);
/**
* @brief Read averaged value
* @param result Pointer to result structure
* @param num_samples Number of samples to average (1-256)
* @return dr_adc_err_t Error code
*/
dr_adc_err_t dr_adc_read_averaged(dr_adc_result_t *result, uint16_t num_samples);
/*==============================================================================
* ECHO DETECTION FUNCTIONS
*============================================================================*/
/**
* @brief Capture echo envelope after piezo burst
* @param buffer Array to store samples (must be pre-allocated)
* @param num_samples Number of samples to capture
* @return dr_adc_err_t Error code
*
* @note Call this immediately after dr_piezo_burst_sw()
*/
dr_adc_err_t dr_adc_capture_echo(uint16_t *buffer, uint16_t num_samples);
/**
* @brief Capture and analyze echo in one call
* @param echo Pointer to echo result structure
* @param config Pointer to capture configuration (NULL for defaults)
* @return dr_adc_err_t Error code
*/
dr_adc_err_t dr_adc_measure_echo(dr_adc_echo_t *echo, const dr_adc_echo_config_t *config);
/**
* @brief Piezo burst + Echo capture in one call
* @param cycles Number of burst cycles (3~7)
* @param delay_us Delay before capture (us)
* @param num_samples Number of samples to capture
* @param echo Pointer to echo result structure
* @return dr_adc_err_t Error code
*/
dr_adc_err_t dr_adc_burst_and_capture(uint8_t cycles, uint16_t delay_us,
uint16_t num_samples, dr_adc_echo_t *echo);
/**
* @brief Get pointer to last captured echo buffer
* @return Pointer to internal buffer (valid until next capture)
* @note Buffer contains num_samples values from last burst_and_capture call
*/
const uint16_t* dr_adc_get_echo_buffer(void);
/**
* @brief Analyze captured echo buffer
* @param buffer Sample buffer
* @param num_samples Number of samples in buffer
* @param echo Pointer to echo result structure
* @param threshold Minimum threshold for valid peak
* @return dr_adc_err_t Error code
*/
dr_adc_err_t dr_adc_analyze_echo(const uint16_t *buffer, uint16_t num_samples,
dr_adc_echo_t *echo, uint16_t threshold);
/**
* @brief Find peak in buffer
* @param buffer Sample buffer
* @param num_samples Number of samples
* @param peak_value Pointer to store peak value
* @param peak_index Pointer to store peak index (can be NULL)
*/
void dr_adc_find_peak(const uint16_t *buffer, uint16_t num_samples,
uint16_t *peak_value, uint16_t *peak_index);
/**
* @brief Calculate baseline (average of first N samples)
* @param buffer Sample buffer
* @param num_samples Number of samples to average for baseline
* @return Baseline value
*/
uint16_t dr_adc_calc_baseline(const uint16_t *buffer, uint16_t num_samples);
/*==============================================================================
* UTILITY FUNCTIONS
*============================================================================*/
/**
* @brief Convert raw value to millivolts
* @param raw_value Raw 12-bit value
* @return Voltage in millivolts
*/
uint32_t dr_adc_raw_to_mv(uint16_t raw_value);
/**
* @brief Set reference voltage
* @param vref_mv Reference voltage in millivolts
*/
void dr_adc_set_vref(uint32_t vref_mv);
/**
* @brief Get reference voltage
* @return Reference voltage in millivolts
*/
uint32_t dr_adc_get_vref(void);
/*==============================================================================
* DEBUG FUNCTIONS
*============================================================================*/
/*==============================================================================
* POWER CONTROL
*============================================================================*/
/*==============================================================================
* BLE TRANSMISSION CALLBACK
*============================================================================*/
/*==============================================================================
* INTEGRATED BURST + CAPTURE + TRANSMIT
*============================================================================*/
/**
* @brief Piezo burst + ADC capture + BLE transmission (all-in-one)
*
* This function performs the complete measurement cycle internally:
* 1. Power on ADC
* 2. Select piezo channel (0~7)
* 3. Execute piezo burst (frequency based on freq_option)
* 4. Capture echo samples (after delay_us) - repeated 'averaging' times
* 5. Average the captured samples (firmware-level noise reduction)
* 6. Analyze peak/baseline
* 7. Transmit data via BLE with proper packet timing
*
* @param freq_option Frequency option: 0=1.8MHz (default), 1=2.1MHz, 2=2.0MHz, 3=1.7MHz
* @param delay_us Delay before capture (us), default 20
* @param num_samples Number of samples to capture (1~200)
* @param cycles Number of burst cycles (3~7), default 5
* @param averaging Number of measurements to average (1~1000), default 1
* @param piezo_ch Piezo channel to use (0~7), default 0
* @param ble_buffer Working buffer for BLE packets (must be >= 240 bytes)
* @return dr_adc_err_t Error code
*
* @note Must call dr_adc_register_ble_tx() before using this function
* @note BLE packets: reb: (header+data merged), red: (continuation, >119 samples only)
* @note Higher averaging reduces noise but increases measurement time (~0.3ms per avg)
*/
dr_adc_err_t dr_adc_burst_capture_transmit(uint8_t freq_option, uint16_t delay_us,
uint16_t num_samples, uint8_t cycles,
uint16_t averaging, uint8_t piezo_ch,
uint8_t *ble_buffer, uint8_t skip_raa);
/**
* @brief Select piezo channel (0~7)
* @param channel Piezo channel number (0~7)
*
* @note Hardware-dependent: requires MUX or individual GPIO control
* Currently uses placeholder - implement based on actual hardware
*/
void dr_piezo_select_channel(uint8_t channel);
/*==============================================================================
* 4-CHANNEL CAPTURE (maa? command support)
*============================================================================*/
/**
* @brief 8-channel echo buffer for maa? command
* Memory: 140 samples × 2 bytes × 8 channels = 2,240 bytes
*/
#define MAA_NUM_CHANNELS 6 /* 4 -> 8 -> 6 jhChun 26.03.17*/
#define MAA_SAMPLES_MAX 200
/**
* @brief Echo data for one channel
*/
typedef struct {
uint16_t samples[MAA_SAMPLES_MAX]; /**< Raw sample data */
uint16_t num_samples; /**< Actual sample count */
} dr_maa_channel_t;
/**
* @brief Capture echo from one channel (no BLE transmission)
*
* Captures averaged echo data for a single channel and stores
* in the provided channel buffer. Does NOT transmit via BLE.
*
* @param freq_option Frequency: 0=1.8MHz, 1=2.1MHz, 2=2.0MHz, 3=1.7MHz
* @param delay_us Delay before capture (us)
* @param num_samples Number of samples (1~200)
* @param cycles Burst cycles (3~7)
* @param averaging Number of averages (1~1000)
* @param piezo_ch Piezo channel (0~7)
* @param out_channel Output channel data structure
* @return dr_adc_err_t Error code
*/
dr_adc_err_t dr_adc_capture_channel_only(uint8_t freq_option, uint16_t delay_us,
uint16_t num_samples, uint8_t cycles,
uint16_t averaging, uint8_t piezo_ch,
dr_maa_channel_t *out_channel);
/**
* @brief Transmit captured channel data via BLE
*
* Sends previously captured channel data using reb+red merged protocol.
* reb: tag(4) + num_samples(2) + data(up to 238B). red: only if > 119 samples.
*
* @param ch_data Pointer to captured channel data
* @param ble_buffer Working buffer for BLE packets (>= 244 bytes)
* @return dr_adc_err_t Error code
*/
dr_adc_err_t dr_adc_transmit_channel(const dr_maa_channel_t *ch_data,
uint8_t *ble_buffer);
/*==============================================================================
* DELTA COMPRESSION (maa? mode=1)
*
* Format:
* Byte 0-1: First sample (16-bit, little endian)
* Byte 2+: Delta values (8-bit signed)
* If delta > 127 or < -127: escape (0x80) + 16-bit value
*
* Expected compression: ~50% (280 bytes -> ~140 bytes)
*============================================================================*/
#define DELTA_ESCAPE_BYTE 0x80 /**< Escape marker for out-of-range delta */
/**
* @brief Compress sample data using delta encoding
*
* @param samples Input sample array (16-bit values)
* @param num_samples Number of samples
* @param out_buffer Output buffer for compressed data
* @param out_size Output: number of bytes written
* @return dr_adc_err_t Error code
*/
dr_adc_err_t dr_adc_delta_compress(const uint16_t *samples, uint16_t num_samples,
uint8_t *out_buffer, uint16_t *out_size);
/**
* @brief Transmit captured channel data via BLE with delta compression
*
* Uses rdb+rdd merged protocol (delta variant of reb+red merged)
*
* @param ch_data Pointer to captured channel data
* @param ble_buffer Working buffer for BLE packets (>= 240 bytes)
* @return dr_adc_err_t Error code
*/
dr_adc_err_t dr_adc_transmit_channel_delta(const dr_maa_channel_t *ch_data,
uint8_t *ble_buffer);
/*==============================================================================
* ASYNC MAA - Non-blocking 8-channel capture
*
* Design: State machine driven by BLE TX complete events
* Flow:
* maa? cmd -> maa_async_start() -> capture all CH -> TX reb:(header+data) -> TX red: (if needed)
* BLE_NUS_EVT_TX_RDY -> maa_async_on_tx_ready() -> TX next packet or next channel
* All done -> TX raa: -> state=IDLE
*============================================================================*/
/** @brief MAA async state machine states */
typedef enum {
MAA_ASYNC_IDLE = 0, /**< Not active */
MAA_ASYNC_CAPTURING, /**< ADC capture in progress */
MAA_ASYNC_TX_HEADER, /**< Sending reb: header+data merged */
MAA_ASYNC_TX_DATA, /**< Sending red: data packets */
MAA_ASYNC_NEXT_CHANNEL, /**< Preparing next channel */
MAA_ASYNC_COMPLETE /**< Sending raa: and finishing */
} maa_async_state_t;
/** @brief MAA async context */
typedef struct {
maa_async_state_t state; /**< Current state */
uint8_t current_ch; /**< Current channel (0~7) */
uint8_t current_pkt; /**< Current packet index */
uint16_t data_offset; /**< Bytes sent so far for current channel */
uint8_t freq_option; /**< Frequency option */
uint16_t delay_us; /**< Capture delay */
uint16_t num_samples; /**< Samples per channel */
uint8_t cycles; /**< Burst cycles */
uint16_t averaging; /**< Averaging count */
uint8_t *ble_buffer; /**< Working buffer for BLE packets */
dr_maa_channel_t channels[MAA_NUM_CHANNELS]; /**< Captured data for each channel */
bool pre_capture_all; /**< true: capture all channels before transmitting (mbb) */
void (*on_complete_cb)(void); /**< callback after async capture completes (NULL = none) */
} maa_async_ctx_t;
/**
* @brief Start async MAA 8-channel capture
*
* Initiates the async state machine. Captures CH0 and begins transmission.
* Subsequent packets are sent when maa_async_on_tx_ready() is called.
*
* @param freq_option Frequency: 0=1.8MHz, 1=2.1MHz, 2=2.0MHz, 3=1.7MHz
* @param delay_us Capture delay (us)
* @param num_samples Samples per channel (1~200)
* @param cycles Burst cycles (3~7)
* @param averaging Averaging count (1~1000)
* @param ble_buffer Working buffer (>= 244 bytes)
* @return dr_adc_err_t DR_ADC_OK if started successfully
*/
dr_adc_err_t maa_async_start(uint8_t freq_option, uint16_t delay_us,
uint16_t num_samples, uint8_t cycles,
uint16_t averaging, uint8_t *ble_buffer);
/**
* @brief Handle BLE TX ready event
*
* Called from BLE_NUS_EVT_TX_RDY handler. Sends next packet or
* transitions to next state.
*
* @return true if more work pending, false if complete or idle
*/
bool maa_async_on_tx_ready(void);
/**
* @brief Check if async MAA is active
* @return true if state != IDLE
*/
bool maa_async_is_busy(void);
/**
* @brief Get current async state (for debugging)
* @return Current state
*/
maa_async_state_t maa_async_get_state(void);
/**
* @brief Abort async MAA operation
*/
void maa_async_abort(void);
/**
* @brief Set auto power-off flag (power off after completion)
*/
void maa_async_set_auto_power(bool on);
void maa_async_set_pre_capture_all(bool on);
/**
* @brief Set async capture completion callback
* Called after raa: is transmitted and power-off. NULL = no callback.
*/
void maa_async_set_on_complete(void (*cb)(void));
#endif /* DR_ADC121S051_H */
project/ble_peripheral/ble_app_bladder_patch/measurement/battery/battery_saadc.c
+292
View File
@@ -0,0 +1,292 @@
/*==============================================================================
* battery_saadc.c - Battery voltage ADC measurement
*
* Measures battery voltage via nRF52840 SAADC on AIN2:
* - 12-bit resolution, 4x oversampling
* - Periodic safety check via battery_loop timer (60 s interval)
* - Sequential: battery -> temperature measurement
* - Auto power-off after 5 consecutive readings below 3500 mV or above 40 C
* - In info4 mode (bulk sensor collection): stores to info_batt
*
* Voltage conversion:
* mV = ADC_VALUE * (600 / 4095) * 6 * 1.42 (resistor divider correction)
*============================================================================*/
#include "sdk_common.h"
#include <stdint.h>
#include <string.h>
#include "nrf.h"
#include "boards.h"
#include "app_error.h"
#include "nrf_drv_saadc.h"
#include "nrf_drv_timer.h"
#include "ble_nus.h"
#include "nrf_log.h"
#include "main.h"
#include "app_timer.h"
#include "battery_saadc.h"
#include "main_timer.h"
#include "tmp235_q1.h"
#include "dr_piezo.h"
#include "debug_print.h"
/* SAADC internal reference voltage (mV, float) */
#define BATTERY_REF_VOLTAGE_IN_MILLIVOLTS 600.0f
/* 1/3 prescaling compensation (input divided by 3, then x2 = total x6) */
#define BATTERY_PRE_SCALING_COMPENSATION 6.0f
/* 12-bit ADC maximum digital value */
#define BATTERY_ADC_RES_12BITS 4095.0f
/* Convert raw ADC value to millivolts */
#define BATTERY_RESULT_IN_MILLI_VOLTS(ADC_VALUE)\
((((ADC_VALUE) * BATTERY_REF_VOLTAGE_IN_MILLIVOLTS) / BATTERY_ADC_RES_12BITS) * BATTERY_PRE_SCALING_COMPENSATION)
/* Single ADC buffer (uninit after each measurement, no double-buffer needed) */
static nrf_saadc_value_t adc_buf;
/* Battery monitoring repeat timer */
APP_TIMER_DEF(m_battery_loop_timer_id);
/* Battery monitoring interval (ms) */
#define BATTERY_LOOP_INTERVAL 60000
/* Safety check consecutive count threshold */
#define SAFETY_CHECK_COUNT 5
/* Low-battery check flag — set by battery_loop, consumed by handler */
bool low_battery_check = false;
/* Safety check mode flag — set by battery handler, consumed by tmp235 handler */
bool safety_check_mode = false;
/* SAADC callback completion flag — used by all_sensors() to wait */
volatile bool battery_saadc_done = false;
/* Safety check: cached battery voltage for use in safety_check_complete() */
static float safety_batt_mv = 0;
/* Safety check: consecutive counters */
static uint8_t low_battery_cnt = 0;
static uint8_t over_temp_cnt = 0;
/* info4: bulk sensor collection mode flag */
extern bool info4;
extern char ble_tx_buffer[BLE_NUS_MAX_DATA_LEN];
extern bool go_device_power_off;
extern which_cmd_t cmd_type_t;
extern uint8_t ble_bin_buffer[BLE_NUS_MAX_DATA_LEN];
/* info4 mode: cached battery voltage (mV) */
volatile uint16_t info_batt;
/* info4 sequential measurement control flags */
extern bool go_temp;
extern bool go_batt;
extern bool motion_raw_data_enabled;
extern bool ble_got_new_data;
extern bool motion_data_once;
/*==============================================================================
* safety_check_complete - Called by tmp235 handler after temperature measurement
*
* Checks both battery voltage and temperature against thresholds.
* 5 consecutive readings exceeding either threshold triggers power OFF.
*============================================================================*/
void safety_check_complete(float temp_c)
{
//DBG_PRINTF("[SAFETY] Batt=%d mV, Temp=%d.%d C\r\n",
// (int)safety_batt_mv, (int)temp_c, ((int)(temp_c * 10)) % 10);
/* Battery check */
if (safety_batt_mv <= LOW_BATTERY_VOLTAGE)
{
low_battery_cnt++;
DBG_PRINTF("[SAFETY] Low batt cnt=%d\r\n", low_battery_cnt);
}
else
{
low_battery_cnt = 0;
}
/* Temperature check */
if (temp_c >= OVER_TEMPERATURE_THRESHOLD)
{
over_temp_cnt++;
DBG_PRINTF("[SAFETY] Over temp cnt=%d\r\n", over_temp_cnt);
}
else
{
over_temp_cnt = 0;
}
/* Power OFF if either threshold exceeded 5 consecutive times */
if (low_battery_cnt >= SAFETY_CHECK_COUNT)
{
low_battery_cnt = 0;
DBG_PRINTF("[SAFETY] Low battery -> Power OFF\r\n");
go_device_power_off = true;
main_timer_start();
}
else if (over_temp_cnt >= SAFETY_CHECK_COUNT)
{
over_temp_cnt = 0;
DBG_PRINTF("[SAFETY] Over temperature -> Power OFF\r\n");
go_device_power_off = true;
main_timer_start();
}
dr_piezo_power_off();
}
/*==============================================================================
* battery_event_handler - SAADC conversion complete callback
*
* Converts the raw ADC value to battery voltage (mV) and then:
* - Low-battery check mode: store voltage, chain temperature measurement
* - info4 mode: store to info_batt (no BLE send)
* - Normal mode: send rsn: response over BLE or UART
*============================================================================*/
void battery_event_handler(nrf_drv_saadc_evt_t const * p_event)
{
if (p_event->type == NRF_DRV_SAADC_EVT_DONE)
{
nrf_saadc_value_t register_val = 0;
float batt_lvl_in_milli_volt_0 = 0;
float batt_lvl_in_milli_volt_1 = 0;
register_val = p_event->data.done.p_buffer[0];
/* Release SAADC — shared with temperature / pressure ADC */
nrf_drv_saadc_channel_uninit(0);
nrf_drv_saadc_uninit();
battery_saadc_done = true;
/* ADC -> mV conversion */
batt_lvl_in_milli_volt_0 = BATTERY_RESULT_IN_MILLI_VOLTS(register_val);
/* Resistor divider correction factor 1.42 */
batt_lvl_in_milli_volt_1 = batt_lvl_in_milli_volt_0 * 1.42f;
/* --- Safety check mode: store voltage, chain temperature measurement --- */
if (low_battery_check == true)
{
low_battery_check = false;
safety_batt_mv = batt_lvl_in_milli_volt_1;
safety_check_mode = true;
/* TMP235 shares piezo TX/RX power rail */
if (!dr_piezo_is_power_on())
{
dr_piezo_power_on();
}
tmp235_voltage_level_meas();
}
/* --- info4 mode: store value for mbb? bulk response --- */
else if (info4 == true)
{
info_batt = batt_lvl_in_milli_volt_1;
}
/* --- Normal mode: send rsn: BLE response --- */
else
{
if (cmd_type_t == CMD_UART)
{
DBG_PRINTF("Tn%d\r\n\r\n", (int)batt_lvl_in_milli_volt_1);
}
else if (cmd_type_t == CMD_BLE)
{
single_format_data(ble_bin_buffer, "rsn:", batt_lvl_in_milli_volt_1);
dr_binary_tx_safe(ble_bin_buffer, 3);
}
}
}
}
/*==============================================================================
* battery_configure - Set up SAADC for battery voltage measurement
*
* AIN2, single-ended, 1/6 gain, 12-bit, 4x oversampling, burst enabled.
* Registers a single buffer (uninit after one conversion).
*============================================================================*/
static void battery_configure(void)
{
nrf_drv_saadc_config_t saadc_config = NRF_DRV_SAADC_DEFAULT_CONFIG;
saadc_config.resolution = NRF_SAADC_RESOLUTION_12BIT;
saadc_config.oversample = NRF_SAADC_OVERSAMPLE_4X;
ret_code_t err_code = nrf_drv_saadc_init(&saadc_config, battery_event_handler);
if (err_code != NRF_SUCCESS)
{
return; /* SAADC busy — skip this cycle, retry next */
}
nrf_saadc_channel_config_t config = NRF_DRV_SAADC_DEFAULT_CHANNEL_CONFIG_SE(NRF_SAADC_INPUT_AIN2);
config.burst = NRF_SAADC_BURST_ENABLED;
config.acq_time = NRF_SAADC_ACQTIME_10US;
err_code = nrf_drv_saadc_channel_init(0, &config);
APP_ERROR_CHECK(err_code);
err_code = nrf_drv_saadc_buffer_convert(&adc_buf, 1);
APP_ERROR_CHECK(err_code);
}
/*==============================================================================
* battery_level_meas - Start a single battery voltage measurement
*
* Configures SAADC and triggers sampling. Result arrives asynchronously
* via battery_event_handler.
*============================================================================*/
void battery_level_meas(void)
{
ret_code_t err_code;
battery_configure();
err_code = nrf_drv_saadc_sample();
APP_ERROR_CHECK(err_code);
}
/*==============================================================================
* battery_loop - Periodic battery monitoring timer callback
*
* Sets the low-battery check flag and starts a measurement.
* Skips if info4 mode is active (SAADC conflict).
*============================================================================*/
void battery_loop(void * p_context)
{
UNUSED_PARAMETER(p_context);
if (info4 == true)
{
return;
}
low_battery_check = true;
battery_level_meas();
}
/* Start the periodic battery monitoring timer. */
void battery_timer_start(void)
{
APP_ERROR_CHECK(app_timer_start(m_battery_loop_timer_id, APP_TIMER_TICKS(BATTERY_LOOP_INTERVAL), NULL));
}
/* Stop the battery monitoring timer. */
void battery_timer_stop(void)
{
APP_ERROR_CHECK(app_timer_stop(m_battery_loop_timer_id));
}
/* Initialise the battery monitoring timer (repeated mode). */
void battery_timer_init(void)
{
APP_ERROR_CHECK(app_timer_create(&m_battery_loop_timer_id, APP_TIMER_MODE_REPEATED, battery_loop));
}
project/ble_peripheral/ble_app_bladder_patch/measurement/battery/battery_saadc.h
+43
View File
@@ -0,0 +1,43 @@
/*==============================================================================
* battery_saadc.h - Battery voltage SAADC measurement interface
*
* Uses the nRF52840 SAADC to measure battery voltage on AIN2.
*
* API:
* battery_level_meas() : one-shot measurement (async, result via callback)
* battery_timer_init/start/stop() : 60-second periodic monitoring timer
*
* Periodic safety check (every 60 s):
* Battery -> Temperature sequential measurement via SAADC.
* Auto power-off after 5 consecutive readings below LOW_BATTERY_VOLTAGE (3500 mV)
* or above OVER_TEMPERATURE_THRESHOLD (40 C).
*============================================================================*/
#ifndef _BATTERY_SAADC_H_
#define _BATTERY_SAADC_H_
/* Low-battery threshold (mV) — 5 consecutive readings below this -> power OFF */
#define LOW_BATTERY_VOLTAGE 3500
/* Over-temperature threshold (deg C) — 5 consecutive readings above this -> power OFF */
#define OVER_TEMPERATURE_THRESHOLD 40.0f
/* SAADC callback completion flag (used by all_sensors() to wait) */
extern volatile bool battery_saadc_done;
/* Safety check mode flag — set by battery_loop, consumed by tmp235 handler */
extern bool safety_check_mode;
/* Called by tmp235 handler when safety check temperature measurement completes */
void safety_check_complete(float temp_c);
/* Start a single async battery measurement. Result handled in callback. */
void battery_level_meas(void);
/* Start the 60-second periodic battery monitoring timer. */
void battery_timer_start(void);
/* Stop the battery monitoring timer. */
void battery_timer_stop(void);
/* Initialise the battery monitoring timer (call once at app start). */
void battery_timer_init(void);
#endif //_BATTERY_SAADC_H_
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/EmbUtils/DataConverter.c
+103
View File
@@ -0,0 +1,103 @@
/*
* ________________________________________________________________________________________________________
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
*
* This software, related documentation and any modifications thereto (collectively “Software”) is subject
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
* and other intellectual property rights laws.
*
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
* from InvenSense is strictly prohibited.
*
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
* OF THE SOFTWARE.
* ________________________________________________________________________________________________________
*/
#include "DataConverter.h"
uint8_t * inv_dc_int32_to_little8(int32_t x, uint8_t * little8)
{
little8[3] = (uint8_t)((x >> 24) & 0xff);
little8[2] = (uint8_t)((x >> 16) & 0xff);
little8[1] = (uint8_t)((x >> 8) & 0xff);
little8[0] = (uint8_t)(x & 0xff);
return little8;
}
uint8_t * inv_dc_int16_to_little8(int16_t x, uint8_t * little8)
{
little8[0] = (uint8_t)(x & 0xff);
little8[1] = (uint8_t)((x >> 8) & 0xff);
return little8;
}
uint8_t * inv_dc_int32_to_big8(int32_t x, uint8_t * big8)
{
big8[0] = (uint8_t)((x >> 24) & 0xff);
big8[1] = (uint8_t)((x >> 16) & 0xff);
big8[2] = (uint8_t)((x >> 8) & 0xff);
big8[3] = (uint8_t)(x & 0xff);
return big8;
}
int32_t inv_dc_little8_to_int32(const uint8_t * little8)
{
int32_t x = 0;
x |= ((int32_t)little8[3] << 24);
x |= ((int32_t)little8[2] << 16);
x |= ((int32_t)little8[1] << 8);
x |= ((int32_t)little8[0]);
return x;
}
int16_t inv_dc_big16_to_int16(uint8_t * data)
{
int16_t result;
result = (*data << 8);
data++;
result |= *data;
return result;
}
int16_t inv_dc_le_to_int16(const uint8_t * little8)
{
uint16_t x = 0;
x |= ((uint16_t)little8[0]);
x |= ((uint16_t)little8[1] << 8);
return (int16_t)x;
}
void inv_dc_sfix32_to_float(const int32_t * in, uint32_t len, uint8_t qx, float * out)
{
uint8_t i;
for(i = 0; i < len; ++i) {
out[i] = (float)in[i] / (1 << qx);
}
}
void inv_dc_float_to_sfix32(const float * in, uint32_t len, uint8_t qx, int32_t * out)
{
uint8_t i;
for(i = 0; i < len; ++i) {
out[i] = (int32_t)((in[i] * (1 << qx)) + ((in[i] >= 0) - 0.5f));
}
}
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/EmbUtils/DataConverter.h
+87
View File
@@ -0,0 +1,87 @@
/*
* ________________________________________________________________________________________________________
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
*
* This software, related documentation and any modifications thereto (collectively “Software”) is subject
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
* and other intellectual property rights laws.
*
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
* from InvenSense is strictly prohibited.
*
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
* OF THE SOFTWARE.
* ________________________________________________________________________________________________________
*/
/** @defgroup DataConverter Data Converter
* @brief Helper functions to convert integer
* @ingroup EmbUtils
* @{
*/
#ifndef _INV_DATA_CONVERTER_H_
#define _INV_DATA_CONVERTER_H_
#include "InvExport.h"
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
/** @brief Converts a 32-bit long to a little endian byte stream
*/
uint8_t INV_EXPORT * inv_dc_int32_to_little8(int32_t x, uint8_t * little8);
/** @brief Converts a 16-bit integer to a little endian byte stream
*/
uint8_t INV_EXPORT * inv_dc_int16_to_little8(int16_t x, uint8_t * little8);
/** @brief Converts a 32-bit long to a big endian byte stream
*/
uint8_t INV_EXPORT * inv_dc_int32_to_big8(int32_t x, uint8_t *big8);
/** @brief Converts a little endian byte stream into a 32-bit integer
*/
int32_t INV_EXPORT inv_dc_little8_to_int32(const uint8_t * little8);
/** @brief Converts a little endian byte stream into a 16-bit integer
*/
int16_t INV_EXPORT inv_dc_le_to_int16(const uint8_t * little8);
/** @brief Converts big endian on 16 bits into an unsigned short
*/
int16_t INV_EXPORT inv_dc_big16_to_int16(uint8_t * data);
/** @brief Converts an array of 32-bit signed fixed-point integers to an array of floats
* @param[in] in Pointer to the first element of the array of 32-bit signed fixed-point integers
* @param[in] len Length of the array
* @param[in] qx Number of bits used to represent the decimal part of the fixed-point integers
* @param[out] out Pointer to the memory area where the output will be stored
*/
void INV_EXPORT inv_dc_sfix32_to_float(const int32_t * in, uint32_t len, uint8_t qx, float * out);
/** @brief Converts an array of floats to an array of 32-bit signed fixed-point integers
* @param[in] in Pointer to the first element of the array of floats
* @param[in] len Length of the array
* @param[in] qx Number of bits used to represent the decimal part of the fixed-point integers
* @param[out] out Pointer to the memory area where the output will be stored
*/
void INV_EXPORT inv_dc_float_to_sfix32(const float * in, uint32_t len, uint8_t qx, int32_t * out);
#ifdef __cplusplus
}
#endif
#endif /* _INV_DATA_CONVERTER_H_ */
/** @} */
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/EmbUtils/ErrorHelper.c
+47
View File
@@ -0,0 +1,47 @@
/*
* ________________________________________________________________________________________________________
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
*
* This software, related documentation and any modifications thereto (collectively “Software”) is subject
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
* and other intellectual property rights laws.
*
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
* from InvenSense is strictly prohibited.
*
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
* OF THE SOFTWARE.
* ________________________________________________________________________________________________________
*/
#include "ErrorHelper.h"
const char * inv_error_str(int error)
{
switch(error) {
case INV_ERROR_SUCCESS: return "Success";
case INV_ERROR: return "Unspecified error";
case INV_ERROR_NIMPL: return "Not implemented";
case INV_ERROR_TRANSPORT: return "Transport error";
case INV_ERROR_TIMEOUT: return "Timeout, action did not complete in time";
case INV_ERROR_SIZE: return "Wrong size error";
case INV_ERROR_OS: return "Operating system failure";
case INV_ERROR_IO: return "Input/Output error";
case INV_ERROR_MEM: return "Bad allocation";
case INV_ERROR_HW: return "Hardware error";
case INV_ERROR_BAD_ARG: return "Invalid arguments";
case INV_ERROR_UNEXPECTED: return "Unexpected error";
case INV_ERROR_FILE: return "Invalid file format";
case INV_ERROR_PATH: return "Invalid file path";
case INV_ERROR_IMAGE_TYPE: return "Unknown image type";
case INV_ERROR_WATCHDOG: return "Watchdog error";
default: return "Unknown error";
}
}
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/EmbUtils/ErrorHelper.h
+51
View File
@@ -0,0 +1,51 @@
/*
* ________________________________________________________________________________________________________
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
*
* This software, related documentation and any modifications thereto (collectively “Software”) is subject
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
* and other intellectual property rights laws.
*
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
* from InvenSense is strictly prohibited.
*
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
* OF THE SOFTWARE.
* ________________________________________________________________________________________________________
*/
/** @defgroup ErrorHelper Error Helper
* @brief Helper functions related to error code
* @ingroup EmbUtils
* @{
*/
#ifndef _INV_ERROR_HELPER_H_
#define _INV_ERROR_HELPER_H_
#include "InvExport.h"
#include "InvError.h"
#ifdef __cplusplus
extern "C" {
#endif
/** @brief Returns string describing error number
* @sa enum inv_error
*/
const char INV_EXPORT * inv_error_str(int error);
#ifdef __cplusplus
}
#endif
#endif /* _INV_ERROR_HELPER_H_ */
/** @} */
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/EmbUtils/InvBasicMath.c
+82
View File
@@ -0,0 +1,82 @@
/*
Copyright (c) 2014-2015 InvenSense Inc. Portions Copyright (c) 2014-2015 Movea. All rights reserved.
This software, related documentation and any modifications thereto (collectively "Software") is subject
to InvenSense and its licensors' intellectual property rights under U.S. and international copyright and
other intellectual property rights laws.
InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
and any use, reproduction, disclosure or distribution of the Software without an express license
agreement from InvenSense is strictly prohibited.
*/
#include "InvBasicMath.h"
#include <limits.h>
unsigned int InvBasicMath_log2u(unsigned int val)
{
unsigned int ret = UINT_MAX;
while (val != 0) {
val >>= 1;
ret++;
}
return ret;
}
int InvBasicMath_isAnOrthonormalMatrix(const float matrix[9])
{
// Check if matrix is orthogonal
// Matrix is orthogonal if transpose(Matrix) x Matrix = Identity
float transpose[9];
float mult[9];
int i, j;
// Compute Transpose(matrix)
for (i = 0; i < 3; i++) {
for(j = 0; j < 3; j++) {
transpose[i*3+j] = matrix[i+j*3];
}
}
// Multiply transpose x matrix
mult[0] = transpose[0]*matrix[0] + transpose[1]*matrix[3] + transpose[2]*matrix[6];
mult[1] = transpose[0]*matrix[1] + transpose[1]*matrix[4] + transpose[2]*matrix[7];
mult[2] = transpose[0]*matrix[2] + transpose[1]*matrix[5] + transpose[2]*matrix[8];
mult[3] = transpose[3]*matrix[0] + transpose[4]*matrix[3] + transpose[5]*matrix[6];
mult[4] = transpose[3]*matrix[1] + transpose[4]*matrix[4] + transpose[5]*matrix[7];
mult[5] = transpose[3]*matrix[2] + transpose[4]*matrix[5] + transpose[5]*matrix[8];
mult[6] = transpose[6]*matrix[0] + transpose[7]*matrix[3] + transpose[8]*matrix[6];
mult[7] = transpose[6]*matrix[1] + transpose[7]*matrix[4] + transpose[8]*matrix[7];
mult[8] = transpose[6]*matrix[2] + transpose[7]*matrix[5] + transpose[8]*matrix[8];
// Check that mult is identity
for (i = 0; i < 3; i++) {
for(j = 0; j < 3; j++) {
if (i == j) {
if (mult[i+j*3] != 1)
return 0;
}
else {
if (mult[i+j*3] != 0)
return 0;
}
}
}
return 1;
}
float InvBasicMath_computeMatrixDeterminant(const float matrix[9])
{
return matrix[0] * (matrix[4]*matrix[8] - matrix[7]*matrix[5])
-matrix[1] * (matrix[3]*matrix[8] - matrix[6]*matrix[5])
+matrix[2] * (matrix[3]*matrix[7] - matrix[4]*matrix[6]);
}
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/EmbUtils/InvBasicMath.h
+94
View File
@@ -0,0 +1,94 @@
/*
Copyright (c) 2014-2015 InvenSense Inc. Portions Copyright (c) 2014-2015 Movea. All rights reserved.
This software, related documentation and any modifications thereto (collectively "Software") is subject
to InvenSense and its licensors' intellectual property rights under U.S. and international copyright and
other intellectual property rights laws.
InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
and any use, reproduction, disclosure or distribution of the Software without an express license
agreement from InvenSense is strictly prohibited.
*/
/** @defgroup InvBasicMath InvBasicMath
@brief This file contains basic (overloadable) math functions and macros
@ingroup EmbUtils
@{
*/
#ifndef _INV_BASIC_MATH_H_
#define _INV_BASIC_MATH_H_
#ifdef __cplusplus
extern "C" {
#endif
/** @brief Return absolute value of argument
*/
#ifndef INV_ABS
# define INV_ABS(a) ((a) < 0 ? -(a) : (a))
#endif
/** @brief Return minimum of two arguments
*/
#ifndef INV_MIN
# define INV_MIN(a, b) ((a) < (b) ? (a) : (b))
#endif
/** @brief Return maximum of two arguments
*/
#ifndef INV_MAX
# define INV_MAX(a, b) ((a) > (b) ? (a) : (b))
#endif
/** @brief Define value for pi
*/
#ifndef INV_PI
# define INV_PI 3.14159265358979
#endif
#ifndef M_PI
# define M_PI INV_PI
#endif
/** @brief Return saturated integer
*/
#ifndef INV_SATURATE
static inline long InvBasicMath_saturatel(long in, long min, long max)
{
if (in > max)
return max;
else if (in < min)
return min;
else
return in;
}
# define INV_SATURATE(a, min, max) InvBasicMath_saturatel(a, min, max)
#endif
/** @brief Compute log2 from integer
*/
#ifndef INV_LOG2
unsigned int InvBasicMath_log2u(unsigned int val);
# define INV_LOG2(a) InvBasicMath_log2u(a)
#endif
/** @brief Check if matrix is orthonormal
* @param [in] matrix 3x3 Matrix to be checked
* @return 1 if it is an orthonormal matrix, 0 otherwise
*/
int InvBasicMath_isAnOrthonormalMatrix(const float matrix[9]);
/** @brief Compute the determinant of the matrix
* @param [in] matrix 3x3 Matrix to be checked
* @return the determinant value
*/
float InvBasicMath_computeMatrixDeterminant(const float matrix[9]);
#ifdef __cplusplus
}
#endif
#endif /* _INV_BASIC_MATH_H_ */
/** @} */
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/EmbUtils/RingBuffer.h
+389
View File
@@ -0,0 +1,389 @@
/*
Copyright (c) 2014-2015 InvenSense Inc. Portions Copyright (c) 2014-2015 Movea. All rights reserved.
This software, related documentation and any modifications thereto (collectively "Software") is subject
to InvenSense and its licensors' intellectual property rights under U.S. and international copyright and
other intellectual property rights laws.
InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
and any use, reproduction, disclosure or distribution of the Software without an express license
agreement from InvenSense is strictly prohibited.
*/
/** \defgroup RingBuffer RingBuffer
\brief Macros to manage static circular buffer of any data type
\ingroup EmbUtils
\{
*/
#ifndef _RING_BUFFER_H_
#define _RING_BUFFER_H_
#include <stdint.h>
#include <string.h>
/** \brief Macro to declare a ring buffer
\param[in] type type of item contained in the ring buffer
\param[in] size number of items that can contain the ring buffer
To improve speed, size should be a power of 2
*/
#define RINGBUFFER_DECLARE(type, size) \
struct { \
uint16_t read, write; \
type buffer[size]; \
}
/** \brief Macro to declare a volatile ring buffer, i.e. modified within an interrupt context
\param[in] type type of item contained in the ring buffer
\param[in] size number of items that can contain the ring buffer
To improve speed, size should be a power of 2
*/
#define RINGBUFFER_VOLATILE_DECLARE(type, size) \
struct { \
volatile uint16_t read, write; \
volatile type buffer[size]; \
}
/** \brief Macro to declare a ring buffer
\param[in] name name of the circular buffer
\param[in] size number of items that can contain the ring buffer
To improve speed, size should be a power of 2
\param[in] type type of item contained in the ring buffer
*/
#define RINGBUFFER(name, size, type) RINGBUFFER_DECLARE(type, size) name
/** \brief Macro to declare a volatile ring buffer, i.e. modified within an interrupt context
\param[in] name name of the circular buffer
\param[in] size number of items that can contain the ring buffer
To improve speed, size should be a power of 2
\param[in] type type of item contained in the ring buffer
*/
#define RINGBUFFER_VOLATILE(name, size, type) RINGBUFFER_VOLATILE_DECLARE(type, size) name
/** \brief Macro to get maximum size of a ring buffer
\param[in] rb pointer to the ring buffer
\return maximum number of items that can contain the ringbuffer
*/
#define RINGBUFFER_MAXSIZE(rb) (sizeof((rb)->buffer)/sizeof((rb)->buffer[0]))
/** \brief Macro to get maximum size of a volatile ring buffer, i.e. modified within an interrupt context
\param[in] rb pointer to the ring buffer
\return maximum number of items that can contain the ringbuffer
*/
#define RINGBUFFER_VOLATILE_MAXSIZE(rb) RINGBUFFER_MAXSIZE(rb)
/** \brief Macro to get current size of a ring buffer
\param[in] rb pointer to the ring buffer
\return current number of items hold in the ringbuffer
*/
#define RINGBUFFER_SIZE(rb) ((uint16_t)((rb)->write - (rb)->read))
static inline uint16_t get_ringbuffer_volatile_size(void * rb)
{
struct { uint16_t read, write; } rb_var;
memcpy(&rb_var, rb, sizeof(rb_var));
return (rb_var.write - rb_var.read);
}
/** \brief Macro to get current size of a volatile ring buffer, i.e. modified within an interrupt context
\param[in] rb pointer to the ring buffer
\return current number of items hold in the ringbuffer
\warning it is advised to put this in a critical section
*/
#define RINGBUFFER_VOLATILE_SIZE(rb) get_ringbuffer_volatile_size(rb)
/** \brief Macro to check if a ring buffer is full
\param[in] rb pointer to the ring buffer
\return 1 if there is no slot left in the ring buffer, 0 otherwise
*/
#define RINGBUFFER_FULL(rb) (RINGBUFFER_SIZE(rb) == RINGBUFFER_MAXSIZE(rb))
/** \brief Macro to check if a volatile ring buffer, i.e. modified within an interrupt context, is full
\param[in] rb pointer to the ring buffer
\return 1 if there is no slot left in the ring buffer, 0 otherwise
\warning it is advised to put this in a critical section
*/
#define RINGBUFFER_VOLATILE_FULL(rb) (RINGBUFFER_VOLATILE_SIZE(rb) == RINGBUFFER_VOLATILE_MAXSIZE(rb))
/** \brief Macro to check if a ring buffer is empty
\param[in] rb pointer to the ring buffer
\return 1 if there is no item in the ring buffer, 0 otherwise
*/
#define RINGBUFFER_EMPTY(rb) (RINGBUFFER_SIZE(rb) == 0)
/** \brief Macro to check if a volatile ring buffer, i.e. modified within an interrupt context, is empty
\param[in] rb pointer to the ring buffer
\return 1 if there is no item in the ring buffer, 0 otherwise
\warning it is advised to put this in a critical section
*/
#define RINGBUFFER_VOLATILE_EMPTY(rb) (RINGBUFFER_VOLATILE_SIZE(rb) == 0)
/** \brief Macro to get number of available slot in a ring buffer
\param[in] rb pointer to the ring buffer
\return number of empty slot in the ring buffer
*/
#define RINGBUFFER_AVAILABLE(rb) (RINGBUFFER_MAXSIZE(rb) - RINGBUFFER_SIZE(rb))
/** \brief Macro to get number of available slot in a volatile ring buffer, i.e. modified within an interrupt context
\param[in] rb pointer to the ring buffer
\return number of empty slot in the ring buffer
\warning it is advised to put this in a critical section
*/
#define RINGBUFFER_VOLATILE_AVAILABLE(rb) (RINGBUFFER_VOLATILE_MAXSIZE(rb) - RINGBUFFER_VOLATILE_SIZE(rb))
/** \brief Macro to clear a ring buffer
\param[in] rb pointer to the ring buffer
*/
#define RINGBUFFER_CLEAR(rb) \
do { \
(rb)->read = 0, (rb)->write = 0; \
} while(0)
/** \brief Macro to clear a volatile ring buffer, i.e. modified within an interrupt context
\param[in] rb pointer to the ring buffer
\warning it is advised to put this in a critical section
*/
#define RINGBUFFER_VOLATILE_CLEAR(rb) RINGBUFFER_CLEAR(rb)
/** \brief Push item by reference
\param[in] rb pointer to the ring buffer
\param[out] refData to available item slot
\warning There is no error checking done.
*/
#define RINGBUFFER_PUSHREF(rb, refData) \
do { \
refData = &(rb)->buffer[(rb)->write % RINGBUFFER_MAXSIZE(rb)]; \
++(rb)->write; \
} while(0)
/** \brief Push item by reference
\param[in] rb pointer to the ring buffer
\param[out] refData to available item slot
\warning There is no error checking done.
\warning it is advised to put this in a critical section
*/
#define RINGBUFFER_VOLATILE_PUSHREF(rb, refData) \
do { \
uint16_t wr_ptr = (rb)->write; \
refData = &(rb)->buffer[wr_ptr % RINGBUFFER_VOLATILE_MAXSIZE(rb)]; \
++(rb)->write; \
} while(0)
/** \brief Return reference to next available slot
No push is performed
\param[in] rb pointer to the ring buffer
\param[out] refData to available item slot
\warning There is no error checking done.
*/
#define RINGBUFFER_GETREFNEXT(rb, refData) \
do { \
refData = &(rb)->buffer[(rb)->write % RINGBUFFER_MAXSIZE(rb)]; \
} while(0)
/** \brief Return reference to next available slot
No push is performed
\param[in] rb pointer to the ring buffer
\param[out] refData to available item slot
\warning There is no error checking done.
\warning it is advised to put this in a critical section
*/
#define RINGBUFFER_VOLATILE_GETREFNEXT(rb, refData) \
do { \
uint16_t wr_ptr = (rb)->write; \
refData = &(rb)->buffer[wr_ptr % RINGBUFFER_VOLATILE_MAXSIZE(rb)]; \
} while(0)
/** \brief Increment write counter
Actually performed a push (assuming data were already copied)
\param[in] rb pointer to the ring buffer
\warning There is no error checking done.
*/
#define RINGBUFFER_INCREMENT(rb, refData) \
do { \
++(rb)->write; \
} while(0)
/** \brief Increment write counter
Actually performed a push (assuming data were already copied)
\param[in] rb pointer to the ring buffer
\warning There is no error checking done.
\warning it is advised to put this in a critical section
*/
#define RINGBUFFER_VOLATILE_INCREMENT(rb, refData) \
do { \
++(rb)->write; \
} while(0)
/** \brief Return reference to youngest item
\param[in] rb pointer to the ring buffer
\param[out] refData reference to youngest item
\warning There is no error checking done.
*/
#define RINGBUFFER_BACK(rb, refData) \
do { \
refData = &(rb)->buffer[((rb)->write-1) % RINGBUFFER_MAXSIZE(rb)]; \
} while(0)
/** \brief Return reference to youngest item
\param[in] rb pointer to the ring buffer
\param[out] refData reference to youngest item
\warning There is no error checking done.
\warning it is advised to put this in a critical section
*/
#define RINGBUFFER_VOLATILE_BACK(rb, refData) \
do { \
uint16_t wr_ptr = (rb)->write; \
refData = &(rb)->buffer[(wr_ptr-1) % RINGBUFFER_VOLATILE_MAXSIZE(rb)]; \
} while(0)
/** \brief Macro to push an item to a ring buffer
\param[in] rb pointer to the ring buffer
\param[in] ptrData pointer to the item to push.
\warning There is no error checking done.
You must check for fullness before pushing data
*/
#define RINGBUFFER_PUSH(rb, ptrData) \
do { \
(rb)->buffer[(rb)->write % RINGBUFFER_MAXSIZE(rb)] = *ptrData; \
++(rb)->write; \
} while(0)
/** \brief Macro to push an item to a volatile ring buffer
\param[in] rb pointer to the ring buffer
\param[in] ptrData pointer to the item to push.
\warning There is no error checking done.
You must check for fullness before pushing data
\warning it is advised to put this in a critical section
*/
#define RINGBUFFER_VOLATILE_PUSH(rb, ptrData) \
do { \
uint16_t wr_ptr = (rb)->write; \
(rb)->buffer[wr_ptr % RINGBUFFER_VOLATILE_MAXSIZE(rb)] = *ptrData; \
++(rb)->write; \
} while(0)
/** \brief Macro to unpush an item to a ring buffer
\param[in] rb pointer to the ring buffer
\param[in] ptrData pointer to placeholder to hold unpushed item
\warning There is no error checking done.
You must check for emptiness before pushing data
*/
#define RINGBUFFER_UNPUSH(rb, ptrData) \
do { \
--(rb)->write; \
*ptrData = (rb)->buffer[(rb)->write % RINGBUFFER_MAXSIZE(rb)]; \
} while(0)
/** \brief Macro to unpush an item to a volatile ring buffer
\param[in] rb pointer to the ring buffer
\param[in] ptrData pointer to placeholder to hold unpushed item
\warning There is no error checking done.
You must check for emptiness before pushing data
\warning it is advised to put this in a critical section
*/
#define RINGBUFFER_VOLATILE_UNPUSH(rb, ptrData) \
do { \
--(rb)->write; \
uint16_t wr_ptr = (rb)->write; \
*ptrData = (rb)->buffer[wr_ptr % RINGBUFFER_VOLATILE_MAXSIZE(rb)]; \
} while(0)
/** \brief Return reference to oldest item
\param[in] rb pointer to the ring buffer
\param[out] refData reference to oldest item
\warning There is no error checking done.
*/
#define RINGBUFFER_FRONT(rb, refData) \
do { \
refData = &(rb)->buffer[(rb)->read % RINGBUFFER_MAXSIZE(rb)]; \
} while(0)
/** \brief Return reference to oldest item
\param[in] rb pointer to the ring buffer
\param[out] refData reference to oldest item
\warning There is no error checking done.
\warning it is advised to put this in a critical section
*/
#define RINGBUFFER_VOLATILE_FRONT(rb, refData) \
do { \
uint16_t rd_ptr = (rb)->read; \
refData = &(rb)->buffer[rd_ptr % RINGBUFFER_VOLATILE_MAXSIZE(rb)]; \
} while(0)
/** \brief Macro to pop an item from a ring buffer
\param[in] rb pointer to the ring buffer
\param[out] ptrData pointer to placeholder to hold popped item
\warning There is no error checking done.
You must check for emptiness before popping data
*/
#define RINGBUFFER_POP(rb, ptrData) \
do { \
*ptrData = (rb)->buffer[(rb)->read % RINGBUFFER_MAXSIZE(rb)]; \
++(rb)->read; \
} while(0)
/** \brief Macro to pop an item from a volatile ring buffer
\param[in] rb pointer to the ring buffer
\param[out] ptrData pointer to placeholder to hold popped item
\warning There is no error checking done.
You must check for emptiness before popping data
\warning it is advised to put this in a critical section
*/
#define RINGBUFFER_VOLATILE_POP(rb, ptrData) \
do { \
uint16_t rd_ptr = (rb)->read; \
*ptrData = (rb)->buffer[rd_ptr % RINGBUFFER_VOLATILE_MAXSIZE(rb)]; \
++(rb)->read; \
} while(0)
/** \brief Macro to pop an item from a ring buffer (data is not copied)
\param[in] rb pointer to the ring buffer
\warning There is no error checking done.
You must check for emptiness before popping data
*/
#define RINGBUFFER_POPNLOSE(rb) \
do { \
++(rb)->read; \
} while(0)
/** \brief Macro to pop an item from a volatile ring buffer (data is not copied)
\param[in] rb pointer to the ring buffer
\warning There is no error checking done.
You must check for emptiness before popping data
\warning it is advised to put this in a critical section
*/
#define RINGBUFFER_VOLATILE_POPNLOSE(rb) \
do { \
++(rb)->read; \
} while(0)
/** \brief Macro to unpop an item to a ring buffer
\param[in] rb pointer to the ring buffer
\param[out] ptrData pointer to to the item to unpop.
\warning There is no error checking done.
You must check for fullness before unpopping data
*/
#define RINGBUFFER_UNPOP(rb, ptrData) \
do { \
--(rb)->read; \
(rb)->buffer[(rb)->read % RINGBUFFER_MAXSIZE(rb)] = *ptrData; \
} while(0)
/** \brief Macro to unpop an item to a volatile ring buffer
\param[in] rb pointer to the ring buffer
\param[out] ptrData pointer to to the item to unpop.
\warning There is no error checking done.
You must check for fullness before unpopping data
\warning it is advised to put this in a critical section
*/
#define RINGBUFFER_VOLATILE_UNPOP(rb, ptrData) \
do { \
--(rb)->read; \
uint16_t rd_ptr = (rb)->read; \
(rb)->buffer[rd_ptr % RINGBUFFER_VOLATILE_MAXSIZE(rb)] = *ptrData; \
} while(0)
#endif
/** \} */
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/InvBool.h
+47
View File
@@ -0,0 +1,47 @@
/*
* ________________________________________________________________________________________________________
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
*
* This software, related documentation and any modifications thereto (collectively “Software”) is subject
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
* and other intellectual property rights laws.
*
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
* from InvenSense is strictly prohibited.
*
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
* OF THE SOFTWARE.
* ________________________________________________________________________________________________________
*/
/** @brief Custom definition for boolean type to avoid compiler discrepancies
* @{
*/
#ifndef _INV_BOOL_H_
#define _INV_BOOL_H_
typedef int inv_bool_t;
#ifndef __cplusplus
#ifndef true
#define true 1
#endif
#ifndef false
#define false 0
#endif
#endif /* __cplusplus */
#endif /* _INV_BOOL_H_ */
/** @} */
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/InvError.h
+64
View File
@@ -0,0 +1,64 @@
/*
* ________________________________________________________________________________________________________
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
*
* This software, related documentation and any modifications thereto (collectively “Software”) is subject
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
* and other intellectual property rights laws.
*
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
* from InvenSense is strictly prohibited.
*
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
* OF THE SOFTWARE.
* ________________________________________________________________________________________________________
*/
/** @defgroup InvError Error code
* @brief Common error code
*
* @ingroup EmbUtils
* @{
*/
#ifndef _INV_ERROR_H_
#define _INV_ERROR_H_
/** @brief Common error code definition
*/
enum inv_error
{
INV_ERROR_SUCCESS = 0, /**< no error */
INV_ERROR = -1, /**< unspecified error */
INV_ERROR_NIMPL = -2, /**< function not implemented for given
arguments */
INV_ERROR_TRANSPORT = -3, /**< error occurred at transport level */
INV_ERROR_TIMEOUT = -4, /**< action did not complete in the expected
time window */
INV_ERROR_SIZE = -5, /**< size/length of given arguments is not
suitable to complete requested action */
INV_ERROR_OS = -6, /**< error related to OS */
INV_ERROR_IO = -7, /**< error related to IO operation */
INV_ERROR_MEM = -9, /**< not enough memory to complete requested
action */
INV_ERROR_HW = -10, /**< error at HW level */
INV_ERROR_BAD_ARG = -11, /**< provided arguments are not good to
perform requested action */
INV_ERROR_UNEXPECTED = -12, /**< something unexpected happened */
INV_ERROR_FILE = -13, /**< cannot access file or unexpected format */
INV_ERROR_PATH = -14, /**< invalid file path */
INV_ERROR_IMAGE_TYPE = -15, /**< error when image type is not managed */
INV_ERROR_WATCHDOG = -16, /**< error when device doesn't respond
to ping */
};
#endif /* _INV_ERROR_H_ */
/** @} */
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/InvExport.h
+39
View File
@@ -0,0 +1,39 @@
/*
* ________________________________________________________________________________________________________
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
*
* This software, related documentation and any modifications thereto (collectively “Software”) is subject
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
* and other intellectual property rights laws.
*
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
* from InvenSense is strictly prohibited.
*
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
* OF THE SOFTWARE.
* ________________________________________________________________________________________________________
*/
#ifndef _INV_IDD_EXPORT_H_
#define _INV_IDD_EXPORT_H_
#if defined(_WIN32)
#if !defined(INV_EXPORT) && defined(INV_DO_DLL_EXPORT)
#define INV_EXPORT __declspec(dllexport)
#elif !defined(INV_EXPORT) && defined(INV_DO_DLL_IMPORT)
#define INV_EXPORT __declspec(dllimport)
#endif
#endif
#if !defined(INV_EXPORT)
#define INV_EXPORT
#endif
#endif /* _INV_IDD_EXPORT_H_ */
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/imu/inv_imu_apex.c
+370
View File
@@ -0,0 +1,370 @@
/*
* ________________________________________________________________________________________________________
* Copyright (c) 2017 InvenSense Inc. All rights reserved.
*
* This software, related documentation and any modifications thereto (collectively "Software") is subject
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
* and other intellectual property rights laws.
*
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
* from InvenSense is strictly prohibited.
*
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
* OF THE SOFTWARE.
* ________________________________________________________________________________________________________
*/
#include "inv_imu_defs.h"
#include "inv_imu_extfunc.h"
#include "inv_imu_driver.h"
#include "inv_imu_apex.h"
int inv_imu_apex_enable_ff(struct inv_imu_device *s)
{
int status = 0;
uint8_t value;
status |= inv_imu_start_dmp(s);
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
value &= ~APEX_CONFIG1_FF_ENABLE_MASK;
value |= (uint8_t)APEX_CONFIG1_FF_ENABLE_EN;
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
return status;
}
int inv_imu_apex_disable_ff(struct inv_imu_device *s)
{
int status = 0;
uint8_t value;
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
value &= ~APEX_CONFIG1_FF_ENABLE_MASK;
value |= (uint8_t)APEX_CONFIG1_FF_ENABLE_DIS;
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
return status;
}
int inv_imu_apex_enable_smd(struct inv_imu_device *s)
{
int status = 0;
uint8_t value;
status |= inv_imu_start_dmp(s);
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
value &= ~APEX_CONFIG1_SMD_ENABLE_MASK;
value |= (uint8_t)APEX_CONFIG1_SMD_ENABLE_EN;
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
return status;
}
int inv_imu_apex_disable_smd(struct inv_imu_device *s)
{
int status = 0;
uint8_t value;
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
value &= ~APEX_CONFIG1_SMD_ENABLE_MASK;
value |= (uint8_t)APEX_CONFIG1_SMD_ENABLE_DIS;
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
return status;
}
int inv_imu_apex_init_parameters_struct(struct inv_imu_device *s, inv_imu_apex_parameters_t *apex_inputs)
{
int status = 0;
(void)s;
/* Default parameters at POR */
apex_inputs->pedo_amp_th = APEX_CONFIG3_PEDO_AMP_TH_62_MG;
apex_inputs->pedo_step_cnt_th = 0x5;
apex_inputs->pedo_step_det_th = 0x2;
apex_inputs->pedo_sb_timer_th = APEX_CONFIG4_PEDO_SB_TIMER_TH_150_SAMPLES;
apex_inputs->pedo_hi_enrgy_th = APEX_CONFIG4_PEDO_HI_ENRGY_TH_104_MG;
apex_inputs->tilt_wait_time = APEX_CONFIG5_TILT_WAIT_TIME_4_S;
apex_inputs->power_save_time = APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_8_S;
apex_inputs->power_save = APEX_CONFIG0_DMP_POWER_SAVE_EN;
apex_inputs->sensitivity_mode = APEX_CONFIG9_SENSITIVITY_MODE_NORMAL;
apex_inputs->low_energy_amp_th = APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_80_MG;
apex_inputs->smd_sensitivity = APEX_CONFIG9_SMD_SENSITIVITY_0;
apex_inputs->ff_debounce_duration = APEX_CONFIG9_FF_DEBOUNCE_DURATION_2000_MS;
apex_inputs->ff_max_duration_cm = APEX_CONFIG12_FF_MAX_DURATION_204_CM;
apex_inputs->ff_min_duration_cm = APEX_CONFIG12_FF_MIN_DURATION_10_CM;
apex_inputs->lowg_peak_th = APEX_CONFIG10_LOWG_PEAK_TH_563_MG;
apex_inputs->lowg_peak_hyst = APEX_CONFIG5_LOWG_PEAK_TH_HYST_156_MG;
apex_inputs->lowg_samples_th = APEX_CONFIG10_LOWG_TIME_TH_1_SAMPLE;
apex_inputs->highg_peak_th = APEX_CONFIG11_HIGHG_PEAK_TH_2500_MG;
apex_inputs->highg_peak_hyst = APEX_CONFIG5_HIGHG_PEAK_TH_HYST_156_MG;
apex_inputs->highg_samples_th = APEX_CONFIG11_HIGHG_TIME_TH_1_SAMPLE;
return status;
}
int inv_imu_apex_configure_parameters(struct inv_imu_device *s, const inv_imu_apex_parameters_t *apex_inputs)
{
int status = 0;
uint8_t data;
uint8_t apexConfig[7];
APEX_CONFIG1_PED_ENABLE_t pedo_state;
APEX_CONFIG1_TILT_ENABLE_t tilt_state;
APEX_CONFIG1_FF_ENABLE_t ff_state;
APEX_CONFIG1_SMD_ENABLE_t smd_state;
/* DMP cannot be configured if it is running, hence make sure all APEX algorithms are off */
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &data);
pedo_state = (APEX_CONFIG1_PED_ENABLE_t)(data & APEX_CONFIG1_PED_ENABLE_MASK);
tilt_state = (APEX_CONFIG1_TILT_ENABLE_t)(data & APEX_CONFIG1_TILT_ENABLE_MASK);
ff_state = (APEX_CONFIG1_FF_ENABLE_t)(data & APEX_CONFIG1_FF_ENABLE_MASK);
smd_state = (APEX_CONFIG1_SMD_ENABLE_t)(data & APEX_CONFIG1_SMD_ENABLE_MASK);
if (pedo_state == APEX_CONFIG1_PED_ENABLE_EN)
return INV_ERROR;
if (tilt_state == APEX_CONFIG1_TILT_ENABLE_EN)
return INV_ERROR;
if (ff_state == APEX_CONFIG1_FF_ENABLE_EN)
return INV_ERROR;
if (smd_state == APEX_CONFIG1_SMD_ENABLE_EN)
return INV_ERROR;
status |= inv_imu_switch_on_mclk(s);
/* Power Save mode and low energy amplitude threshold (for Pedometer in Slow Walk mode) */
/* APEX_CONFIG2_MREG1 */
apexConfig[0] = (uint8_t)apex_inputs->power_save_time
| (uint8_t)apex_inputs->low_energy_amp_th;
/* Pedometer parameters */
/* APEX_CONFIG3_MREG1 */
apexConfig[1] = (uint8_t)apex_inputs->pedo_amp_th
| (apex_inputs->pedo_step_cnt_th & APEX_CONFIG3_PED_STEP_CNT_TH_SEL_MASK);
/* APEX_CONFIG4_MREG1 */
apexConfig[2] = ((apex_inputs->pedo_step_det_th << APEX_CONFIG4_PED_STEP_DET_TH_SEL_POS)
& APEX_CONFIG4_PED_STEP_DET_TH_SEL_MASK)
| (uint8_t)apex_inputs->pedo_sb_timer_th
| (uint8_t)apex_inputs->pedo_hi_enrgy_th;
/* Tilt, Lowg and highg parameters */
/* APEX_CONFIG5_MREG1 */
apexConfig[3] = (uint8_t)apex_inputs->tilt_wait_time
| (uint8_t)apex_inputs->lowg_peak_hyst
| (uint8_t)apex_inputs->highg_peak_hyst;
status |= inv_imu_write_reg(s, APEX_CONFIG2_MREG1, 4, &apexConfig[0]);
/* APEX_CONFIG0 */
status |= inv_imu_read_reg(s, APEX_CONFIG0, 1, &apexConfig[0]);
apexConfig[0] &= ~APEX_CONFIG0_DMP_POWER_SAVE_EN_MASK;
apexConfig[0] |= apex_inputs->power_save;
status |= inv_imu_write_reg(s, APEX_CONFIG0, 1, &apexConfig[0]);
/* free fall parameter, SMD parameter and parameters for Pedometer in Slow Walk mode */
/* APEX_CONFIG9_MREG1 */
apexConfig[0] = (uint8_t)apex_inputs->ff_debounce_duration
| (uint8_t)apex_inputs->smd_sensitivity
| (uint8_t)apex_inputs->sensitivity_mode;
/* Lowg and highg parameters and free fall parameters */
/* APEX_CONFIG10_MREG1 */
apexConfig[1] = (uint8_t)apex_inputs->lowg_peak_th
| (uint8_t)apex_inputs->lowg_samples_th;
/* APEX_CONFIG11_MREG1 */
apexConfig[2] = (uint8_t)apex_inputs->highg_peak_th
| (uint8_t)apex_inputs->highg_samples_th;
status |= inv_imu_write_reg(s, APEX_CONFIG9_MREG1, 3, &apexConfig[0]);
/* APEX_CONFIG12_MREG1 */
apexConfig[0] = (uint8_t)apex_inputs->ff_max_duration_cm
| (uint8_t)apex_inputs->ff_min_duration_cm;
status |= inv_imu_write_reg(s, APEX_CONFIG12_MREG1, 1, &apexConfig[0]);
status |= inv_imu_switch_off_mclk(s);
return status;
}
int inv_imu_apex_get_parameters(struct inv_imu_device *s, inv_imu_apex_parameters_t *apex_params)
{
int status = 0;
uint8_t data[7];
uint8_t value;
status |= inv_imu_read_reg(s, APEX_CONFIG0, 1, &value);
apex_params->power_save = (APEX_CONFIG0_DMP_POWER_SAVE_t)(value & APEX_CONFIG0_DMP_POWER_SAVE_EN_MASK);
/* Access continuous config registers (CONFIG2-CONFIG11) */
status |= inv_imu_read_reg(s, APEX_CONFIG2_MREG1, sizeof(data), &data[0]);
/* Get params from apex_config2 : dmp_power_save_time and low_energy_amp_th */
apex_params->power_save_time = (APEX_CONFIG2_DMP_POWER_SAVE_TIME_t)
(data[0] & APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_MASK);
apex_params->low_energy_amp_th = (APEX_CONFIG2_LOW_ENERGY_AMP_TH_t)
(data[0] & APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_MASK);
/* Get params from apex_config3 : pedo_amp_th and pedo_step_cnt_th */
apex_params->pedo_amp_th = (APEX_CONFIG3_PEDO_AMP_TH_t)
(data[1] & APEX_CONFIG3_PED_AMP_TH_SEL_MASK);
apex_params->pedo_step_cnt_th = (data[1] & APEX_CONFIG3_PED_STEP_CNT_TH_SEL_MASK)
>> APEX_CONFIG3_PED_STEP_CNT_TH_SEL_POS;
/* Get params from apex_config4 : pedo_step_det_th, pedo_sb_timer_th and pedo_hi_enrgy_th */
apex_params->pedo_step_det_th = (data[2] & APEX_CONFIG4_PED_STEP_DET_TH_SEL_MASK)
>> APEX_CONFIG4_PED_STEP_DET_TH_SEL_POS;
apex_params->pedo_sb_timer_th = (APEX_CONFIG4_PEDO_SB_TIMER_TH_t)
(data[2] & APEX_CONFIG4_PED_SB_TIMER_TH_SEL_MASK);
apex_params->pedo_hi_enrgy_th = (APEX_CONFIG4_PEDO_HI_ENRGY_TH_t)
(data[2] & APEX_CONFIG4_PED_HI_EN_TH_SEL_MASK);
/* Get params from apex_config5 : tilt_wait_time, lowg_peak_hyst and highg_peak_hyst */
apex_params->tilt_wait_time = (APEX_CONFIG5_TILT_WAIT_TIME_t)
(data[3] & APEX_CONFIG5_TILT_WAIT_TIME_SEL_MASK);
apex_params->lowg_peak_hyst = (APEX_CONFIG5_LOWG_PEAK_TH_HYST_t)
(data[3] & APEX_CONFIG5_LOWG_PEAK_TH_HYST_SEL_MASK);
apex_params->highg_peak_hyst = (APEX_CONFIG5_HIGHG_PEAK_TH_HYST_t)
(data[3] & APEX_CONFIG5_HIGHG_PEAK_TH_HYST_SEL_MASK);
/* Get params from apex_config9 : ff_debounce_duration, smd_sensitivity and sensitivity_mode */
apex_params->ff_debounce_duration = (APEX_CONFIG9_FF_DEBOUNCE_DURATION_t)
(data[4] & APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_MASK);
apex_params->smd_sensitivity = (APEX_CONFIG9_SMD_SENSITIVITY_t)
(data[4] & APEX_CONFIG9_SMD_SENSITIVITY_SEL_MASK);
apex_params->sensitivity_mode = (APEX_CONFIG9_SENSITIVITY_MODE_t)
(data[4] & APEX_CONFIG9_SENSITIVITY_MODE_MASK);
/* Get params from apex_config10 : lowg_peak_th and lowg_samples_th */
apex_params->lowg_peak_th = (APEX_CONFIG10_LOWG_PEAK_TH_t)
(data[5] & APEX_CONFIG10_LOWG_PEAK_TH_SEL_MASK);
apex_params->lowg_samples_th = (APEX_CONFIG10_LOWG_TIME_TH_SAMPLES_t)
(data[5] & APEX_CONFIG10_LOWG_TIME_TH_SEL_MASK);
/* Get params from apex_config11 : highg_peak_th and highg_samples_th */
apex_params->highg_peak_th = (APEX_CONFIG11_HIGHG_PEAK_TH_t)
(data[6] & APEX_CONFIG11_HIGHG_PEAK_TH_SEL_MASK);
apex_params->highg_samples_th = (APEX_CONFIG11_HIGHG_TIME_TH_SAMPLES_t)
(data[6] & APEX_CONFIG11_HIGHG_TIME_TH_SEL_MASK);
/* Access apex reg 12 */
status |= inv_imu_read_reg(s, APEX_CONFIG12_MREG1, 1, &data[0]);
/* Get params from apex_config12 : ff_max_duration_cm and ff_min_duration_cm */
apex_params->ff_max_duration_cm = (APEX_CONFIG12_FF_MAX_DURATION_t)
(data[0] & APEX_CONFIG12_FF_MAX_DURATION_SEL_MASK);
apex_params->ff_min_duration_cm = (APEX_CONFIG12_FF_MIN_DURATION_t)
(data[0] & APEX_CONFIG12_FF_MIN_DURATION_SEL_MASK);
return status;
}
int inv_imu_apex_set_frequency(struct inv_imu_device *s, const APEX_CONFIG1_DMP_ODR_t frequency)
{
uint8_t value;
int status = 0;
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
value &= ~APEX_CONFIG1_DMP_ODR_MASK;
value |= frequency;
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
return status;
}
int inv_imu_apex_enable_pedometer(struct inv_imu_device *s)
{
int status = 0;
uint8_t value;
status |= inv_imu_start_dmp(s);
/* Enable Pedometer */
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
value &= ~APEX_CONFIG1_PED_ENABLE_MASK;
value |= (uint8_t)APEX_CONFIG1_PED_ENABLE_EN;
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
return status;
}
int inv_imu_apex_disable_pedometer(struct inv_imu_device *s)
{
int status = 0;
uint8_t value;
/* Disable Pedometer */
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
value &= ~APEX_CONFIG1_PED_ENABLE_MASK;
value |= (uint8_t)APEX_CONFIG1_PED_ENABLE_DIS;
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
return status;
}
int inv_imu_apex_enable_tilt(struct inv_imu_device *s)
{
int status = 0;
uint8_t value;
status |= inv_imu_start_dmp(s);
/* Enable Tilt */
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
value &= ~APEX_CONFIG1_TILT_ENABLE_MASK;
value |= (uint8_t)APEX_CONFIG1_TILT_ENABLE_EN;
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
return status;
}
int inv_imu_apex_disable_tilt(struct inv_imu_device *s)
{
int status = 0;
uint8_t value;
/* Disable Tilt */
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
value &= ~APEX_CONFIG1_TILT_ENABLE_MASK;
value |= (uint8_t)APEX_CONFIG1_TILT_ENABLE_DIS;
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
return status;
}
int inv_imu_apex_get_data_activity(struct inv_imu_device *s, inv_imu_apex_step_activity_t *apex_activity)
{
uint8_t data[4];
int status = inv_imu_read_reg(s, APEX_DATA0, 4, data);
apex_activity->step_cnt = data[1] << 8 | data[0];
apex_activity->step_cadence = data[2];
apex_activity->activity_class = data[3] & APEX_DATA3_ACTIVITY_CLASS_MASK;
return status;
}
int inv_imu_apex_get_data_free_fall(struct inv_imu_device *s, uint16_t *freefall_duration)
{
uint8_t data[2];
int status = inv_imu_read_reg(s, APEX_DATA4, 2, &data[0]);
*freefall_duration = (data[1] << 8) | data[0];
return status;
}
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/imu/inv_imu_apex.h
+185
View File
@@ -0,0 +1,185 @@
/*
* ________________________________________________________________________________________________________
* Copyright (c) 2017 InvenSense Inc. All rights reserved.
*
* This software, related documentation and any modifications thereto (collectively "Software") is subject
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
* and other intellectual property rights laws.
*
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
* from InvenSense is strictly prohibited.
*
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
* OF THE SOFTWARE.
* ________________________________________________________________________________________________________
*/
/** @defgroup DriverApex IMU driver high level functions related to APEX and the DMP
* @brief High-level function to setup an IMU device
* @ingroup Driver
* @{
*/
/** @file inv_imu_apex.h
* High-level function to setup an IMU device
*/
#ifndef _INV_IMU_APEX_H_
#define _INV_IMU_APEX_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "inv_imu_defs.h"
#include "InvError.h"
#include <stdint.h>
#include <string.h>
/* Forward declarations */
struct inv_imu_device;
/** @brief IMU APEX inputs parameters definition
*/
typedef struct {
APEX_CONFIG3_PEDO_AMP_TH_t pedo_amp_th;
uint8_t pedo_step_cnt_th;
uint8_t pedo_step_det_th;
APEX_CONFIG4_PEDO_SB_TIMER_TH_t pedo_sb_timer_th;
APEX_CONFIG4_PEDO_HI_ENRGY_TH_t pedo_hi_enrgy_th;
APEX_CONFIG5_TILT_WAIT_TIME_t tilt_wait_time;
APEX_CONFIG2_DMP_POWER_SAVE_TIME_t power_save_time;
APEX_CONFIG0_DMP_POWER_SAVE_t power_save;
APEX_CONFIG9_SENSITIVITY_MODE_t sensitivity_mode;
APEX_CONFIG2_LOW_ENERGY_AMP_TH_t low_energy_amp_th;
APEX_CONFIG9_SMD_SENSITIVITY_t smd_sensitivity;
APEX_CONFIG9_FF_DEBOUNCE_DURATION_t ff_debounce_duration;
APEX_CONFIG12_FF_MAX_DURATION_t ff_max_duration_cm;
APEX_CONFIG12_FF_MIN_DURATION_t ff_min_duration_cm;
APEX_CONFIG10_LOWG_PEAK_TH_t lowg_peak_th;
APEX_CONFIG5_LOWG_PEAK_TH_HYST_t lowg_peak_hyst;
APEX_CONFIG10_LOWG_TIME_TH_SAMPLES_t lowg_samples_th;
APEX_CONFIG11_HIGHG_PEAK_TH_t highg_peak_th;
APEX_CONFIG5_HIGHG_PEAK_TH_HYST_t highg_peak_hyst;
APEX_CONFIG11_HIGHG_TIME_TH_SAMPLES_t highg_samples_th;
} inv_imu_apex_parameters_t;
/** @brief APEX pedometer outputs
*/
typedef struct inv_imu_apex_step_activity {
uint16_t step_cnt; /**< Number of steps taken */
uint8_t step_cadence; /**< Walk/run cadence in number of samples.
Format is u6.2. E.g, At 50Hz and 2Hz walk frequency, if the cadency is 25 samples.
The register will output 100. */
uint8_t activity_class; /**< Detected activity unknown (0), walk (1) or run (2) */
} inv_imu_apex_step_activity_t;
/** @brief Enable Free Fall.
* @return 0 on success, negative value on error.
*/
int inv_imu_apex_enable_ff(struct inv_imu_device *s);
/** @brief Disable Free Fall.
* @return 0 on success, negative value on error.
*/
int inv_imu_apex_disable_ff(struct inv_imu_device *s);
/** @brief Enable Significant Motion Detection.
* note : SMD requests to have the accelerometer enabled to work.
* To have good performance, it's recommended to set accelerometer ODR (Output Data Rate) to 20ms
* and the accelerometer in Low Power Mode.
* @return 0 on success, negative value on error.
*/
int inv_imu_apex_enable_smd(struct inv_imu_device *s);
/** @brief Disable Significant Motion Detection.
* @return 0 on success, negative value on error.
*/
int inv_imu_apex_disable_smd(struct inv_imu_device *s);
/** @brief Fill the APEX parameters structure with all the default parameters for APEX algorithms (pedometer, tilt)
* @param[out] apex_inputs Default input parameters. See @sa inv_imu_apex_parameters_t
* @return 0 on success, negative value on error.
*/
int inv_imu_apex_init_parameters_struct(struct inv_imu_device *s, inv_imu_apex_parameters_t *apex_inputs);
/** @brief Configures DMP parameters for APEX algorithms (pedometer, tilt, lowg, highg).
* This programmable parameters will be decoded and propagate to the SRAM to be executed at DMP start.
* @param[in] apex_inputs The requested input parameters. See @sa inv_imu_apex_parameters_t
* @warning APEX inputs can't change on the fly, this API should be called before enabling any APEX features.
* @warning APEX configuration can't be done too frequently, but only once every 10ms.
* Otherwise it can create unknown behavior.
* @return 0 on success, negative value on error.
*/
int inv_imu_apex_configure_parameters(struct inv_imu_device *s, const inv_imu_apex_parameters_t *apex_inputs);
/** @brief Returns current DMP parameters for APEX algorithms (pedometer, tilt).
* @param[out] apex_params The current parameter, fetched from registers. See @sa inv_imu_apex_parameters_t
* @return 0 on success, negative value on error.
*/
int inv_imu_apex_get_parameters(struct inv_imu_device *s, inv_imu_apex_parameters_t *apex_params);
/** @brief Configure DMP Output Data Rate for APEX algorithms (pedometer, tilt)
* @param[in] frequency The requested frequency.
* @sa APEX_CONFIG1_DMP_ODR_t
* @warning DMP_ODR can change on the fly, and the DMP code will accommodate necessary modifications
* @warning The user needs to take care to set Accel frequency >= DMP frequency. This is a hard constraint
since HW will not handle incorrect setting.
* @return 0 on success, negative value on error.
*/
int inv_imu_apex_set_frequency(struct inv_imu_device *s, const APEX_CONFIG1_DMP_ODR_t frequency);
/** @brief Enable APEX algorithm Pedometer.
* note : Pedometer request to have the accelerometer enabled to works
* with accelerometer frequency less than dmp frequency.
* @return 0 on success, negative value on error.
* @warning Pedometer must be turned OFF to reconfigure it
*/
int inv_imu_apex_enable_pedometer(struct inv_imu_device *s);
/** @brief Disable APEX algorithm Pedometer.
* @return 0 on success, negative value on error.
*/
int inv_imu_apex_disable_pedometer(struct inv_imu_device *s);
/** @brief Enable APEX algorithm Tilt.
* note : Tilt request to have the accelerometer enabled to works
* with accelerometer frequency less than dmp frequency.
* @return 0 on success, negative value on error.
*/
int inv_imu_apex_enable_tilt(struct inv_imu_device *s);
/** @brief Disable APEX algorithm Tilt.
* @return 0 on success, negative value on error.
*/
int inv_imu_apex_disable_tilt(struct inv_imu_device *s);
/** @brief Retrieve APEX pedometer outputs and format them
* @param[out] apex_activity Apex step and activity data value.
* @return 0 in case of success, negative value on error. See enum inv_error
*/
int inv_imu_apex_get_data_activity(struct inv_imu_device *s, inv_imu_apex_step_activity_t *apex_activity);
/** @brief Retrieve APEX free fall outputs and format them
* @param[out] Free fall duration in number of sample.
* @return 0 in case of success, negative value on error. See enum inv_error
*/
int inv_imu_apex_get_data_free_fall(struct inv_imu_device *s, uint16_t *freefall_duration);
#ifdef __cplusplus
}
#endif
#endif /* _INV_IMU_APEX_H_ */
/** @} */
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/imu/inv_imu_defs.h
+1011
View File
File diff suppressed because it is too large Load Diff
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/imu/inv_imu_driver.c
+1619
View File
File diff suppressed because it is too large Load Diff
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/imu/inv_imu_driver.h
+520
View File
@@ -0,0 +1,520 @@
/*
* ________________________________________________________________________________________________________
* Copyright (c) 2017 InvenSense Inc. All rights reserved.
*
* This software, related documentation and any modifications thereto (collectively "Software") is subject
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
* and other intellectual property rights laws.
*
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
* from InvenSense is strictly prohibited.
*
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
* OF THE SOFTWARE.
* ________________________________________________________________________________________________________
*/
/** @defgroup Driver IMU driver high level functions
* @brief High-level function to setup an IMU device
* @ingroup DriverIcm
* @{
*/
/** @file inv_imu_driver.h
* High-level function to setup an IMU device
*/
#ifndef _INV_IMU_DRIVER_H_
#define _INV_IMU_DRIVER_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "inv_imu_defs.h"
#include "inv_imu_transport.h"
#include "InvError.h"
#include <stdint.h>
#include <string.h>
/** @brief IMU max FSR values for accel and gyro
* Dependent on chip
*/
#define ACCEL_CONFIG0_FS_SEL_MAX ACCEL_CONFIG0_FS_SEL_16g
#define GYRO_CONFIG0_FS_SEL_MAX GYRO_CONFIG0_FS_SEL_2000dps
#define ACCEL_OFFUSER_MAX_MG 1000
#define GYRO_OFFUSER_MAX_DPS 64
/** @brief IMU maximum buffer size mirrored from FIFO at polling time
* @warning fifo_idx type variable must be large enough to parse the FIFO_MIRRORING_SIZE
*/
#define FIFO_MIRRORING_SIZE 16 * 258 // packet size * max_count = 4kB
/** @brief IMU Accelerometer start-up time before having correct data
*/
#define ACC_STARTUP_TIME_US 10000
/** @brief IMU Gyroscope start-up time before having correct data
*/
#define GYR_STARTUP_TIME_US 70000
/** @brief IMU Gyroscope power off to power on duration
*/
#define GYR_POWER_OFF_DUR_US 20000
/** @brief Sensor identifier for UI control function
*/
enum inv_imu_sensor {
INV_SENSOR_ACCEL, /**< Accelerometer */
INV_SENSOR_GYRO, /**< Gyroscope */
INV_SENSOR_FSYNC_EVENT, /**< FSYNC */
INV_SENSOR_TEMPERATURE, /**< Chip temperature */
INV_SENSOR_DMP_PEDOMETER_EVENT, /**< Pedometer: step detected */
INV_SENSOR_DMP_PEDOMETER_COUNT, /**< Pedometer: step counter */
INV_SENSOR_DMP_TILT, /**< Tilt */
INV_SENSOR_DMP_FF, /**< FreeFall */
INV_SENSOR_DMP_LOWG, /**< Low G */
INV_SENSOR_DMP_SMD, /**< Significant Motion Detection */
INV_SENSOR_MAX
};
/** @brief Configure Fifo usage
*/
typedef enum {
INV_IMU_FIFO_DISABLED = 0, /**< Fifo is disabled and data source is sensors registers */
INV_IMU_FIFO_ENABLED = 1, /**< Fifo is used as data source */
}INV_IMU_FIFO_CONFIG_t;
/** @brief Sensor event structure definition
*/
typedef struct {
int sensor_mask;
uint16_t timestamp_fsync;
int16_t accel[3];
int16_t gyro[3];
int16_t temperature;
int8_t accel_high_res[3];
int8_t gyro_high_res[3];
} inv_imu_sensor_event_t;
/** @brief IMU driver states definition
*/
struct inv_imu_device {
struct inv_imu_transport transport; /**< Transport layer
Must be the first one of struct inv_imu_device */
void (*sensor_event_cb)(inv_imu_sensor_event_t *event); /**< callback executed by:
inv_imu_get_data_from_fifo (if FIFO is used)
inv_imu_get_data_from_registers (if FIFO isn't used)
May be NULL if above API are not used by application */
uint8_t fifo_data[FIFO_MIRRORING_SIZE]; /**< FIFO mirroring memory area */
uint8_t dmp_is_on; /**< DMP started status */
uint8_t endianness_data; /**< Data endianness configuration */
uint8_t fifo_highres_enabled; /**< Highres mode configuration */
INV_IMU_FIFO_CONFIG_t fifo_is_used; /**< FIFO configuration */
uint64_t gyro_start_time_us; /**< Gyro start time used to discard first samples */
uint64_t accel_start_time_us; /**< Accel start time used to discard first samples */
uint64_t gyro_power_off_tmst; /**< Gyro power off time */
};
/* Interrupt enum state for INT1, INT2, and IBI */
typedef enum {
INV_IMU_DISABLE = 0,
INV_IMU_ENABLE
} inv_imu_interrupt_value;
/** @brief Interrupt definition
*/
typedef struct {
inv_imu_interrupt_value INV_UI_FSYNC;
inv_imu_interrupt_value INV_UI_DRDY;
inv_imu_interrupt_value INV_FIFO_THS;
inv_imu_interrupt_value INV_FIFO_FULL;
inv_imu_interrupt_value INV_SMD;
inv_imu_interrupt_value INV_WOM_X;
inv_imu_interrupt_value INV_WOM_Y;
inv_imu_interrupt_value INV_WOM_Z;
inv_imu_interrupt_value INV_FF;
inv_imu_interrupt_value INV_LOWG;
inv_imu_interrupt_value INV_STEP_DET;
inv_imu_interrupt_value INV_STEP_CNT_OVFL;
inv_imu_interrupt_value INV_TILT_DET;
} inv_imu_interrupt_parameter_t;
/** @brief Configure the serial interface used to access the device and execute hardware initialization.
*
* This functions first configures serial interface passed in parameter to make sure device
* is accessible both in read and write. Thus no serial access should be done before
* successfully executing the present function.
*
* Then if requested serial interface is a primary interface (aka UI interface or AP
* interface), this function initializes the device using the following hardware settings:
* - set timestamp resolution to 16us
* - enable FIFO mechanism with the following configuration:
* - FIFO record mode i.e FIFO count unit is packet
* - FIFO snapshot mode i.e drop the data when the FIFO overflows
* - Timestamp is logged in FIFO
* - Little Endian fifo_count and fifo_data
* - generate FIFO threshold interrupt when packet count reaches FIFO watermark
* - set FIFO watermark to 1 packet
* - enable temperature and timestamp data to go to FIFO
*
*
* @param[in] s driver structure. Note that first field of this structure MUST be a struct
* inv_imu_serif.
*
* @param[in] serif pointer on serial interface structure to be used to access inv_device.
*
* @param[in] sensor_event_cb callback executed by inv_imu_get_data_from_fifo function
* each time it extracts some valid data from fifo. Or inv_imu_get_data_from_registers read data
* from register. Thus this parameter is optional as long
* as inv_imu_get_data_from_fifo/inv_imu_get_data_from_registers function is not used.
*
* @return 0 on success, negative value on error.
*/
int inv_imu_init(struct inv_imu_device *s,
struct inv_imu_serif *serif,
void (*sensor_event_cb)(inv_imu_sensor_event_t *event));
/** @brief Perform a soft reset of the device
* @return 0 on success, negative value on error.
*/
int inv_imu_device_reset(struct inv_imu_device *s);
/** @brief return WHOAMI value
* @param[out] who_am_i WHOAMI for device
* @return 0 on success, negative value on error
*/
int inv_imu_get_who_am_i(struct inv_imu_device *s, uint8_t *who_am_i);
/** @brief Enable/put accel in low power mode
* @return 0 on success, negative value on error.
* @details
* It enables accel and gyro data in the FIFO (so
* the packet format is 16 bytes). If called first,
* the configuration will be applied, otherwise it
* will be ignored if the FIFO is not empty (but since
* the new configuration is identical it is not a issue).
* @warning inv_device::register_cache::pwr_mgmt0_reg is modified by this function
*/
int inv_imu_enable_accel_low_power_mode(struct inv_imu_device *s);
/** @brief Enable/put accel in low noise mode
* @return 0 on success, negative value on error.
* @details
* It enables accel and gyro data in the FIFO (so
* the packet format is 16 bytes). If called first,
* the configuration will be applied, otherwise it
* will be ignored if the FIFO is not empty (but since
* the new configuration is identical it is not a issue).
* @warning inv_device::register_cache::pwr_mgmt0_reg is modified by this function
*/
int inv_imu_enable_accel_low_noise_mode(struct inv_imu_device *s);
/** @brief Disable all 3 axes of accel
* @return 0 on success, negative value on error.
* @details
* If both accel and gyro are turned off as a result of this
* function, they will also be removed from the FIFO and a
* FIFO reset will be performed (to guarantee no side effects
* until the next enable sensor call)
* @warning inv_device::register_cache::pwr_mgmt0_reg is modified by this function
*/
int inv_imu_disable_accel(struct inv_imu_device *s);
/** @brief Enable/put gyro in low noise mode
* @return 0 on success, negative value on error.
* @details
* It enables gyro and accel data in the FIFO (so
* the packet format is 16 bytes). If called first,
* the configuration will be applied, otherwise it
* will be ignored if the FIFO is not empty (but since
* the new configuration is identical it is not a issue).
* @warning inv_device::register_cache::pwr_mgmt0_reg is modified by this function
*/
int inv_imu_enable_gyro_low_noise_mode(struct inv_imu_device *s);
/** @brief Disable all 3 axes of gyro
* @return 0 on success, negative value on error.
* @details
* If both accel and gyro are turned off as a result of this
* function, they will also be removed from the FIFO and a
* FIFO reset will be performed (to guarantee no side effects
* until the next enable sensor call)
* @warning inv_device::register_cache::pwr_mgmt0_reg is modified by this function
*/
int inv_imu_disable_gyro(struct inv_imu_device *s);
/** @brief Enable fsync tagging functionality.
* In details it:
* - enables fsync
* - enables timestamp to registers. Once fsync is enabled fsync counter is pushed to
* fifo instead of timestamp. So timestamp is made available in registers. Note that
* this increase power consumption.
* - enables fsync related interrupt
* @return 0 on success, negative value on error.
*/
int inv_imu_enable_fsync(struct inv_imu_device *s);
/** @brief Disable fsync tagging functionality.
* In details it:
* - disables fsync
* - disables timestamp to registers. Once fsync is disabled timestamp is pushed to fifo
* instead of fsync counter. So in order to decrease power consumption, timestamp is no
* more available in registers.
* - disables fsync related interrupt
* @return 0 on success, negative value on error.
*/
int inv_imu_disable_fsync(struct inv_imu_device *s);
/** @brief Configure which interrupt source can trigger INT1.
* @param[in] interrupt_to_configure structure with the corresponding state to manage INT1.
* @return 0 on success, negative value on error.
*/
int inv_imu_set_config_int1(struct inv_imu_device *s,
inv_imu_interrupt_parameter_t *interrupt_to_configure);
/** @brief Retrieve interrupts configuration.
* @param[in] interrupt_to_configure structure with the corresponding state to manage INT1.
* @return 0 on success, negative value on error.
*/
int inv_imu_get_config_int1(struct inv_imu_device *s,
inv_imu_interrupt_parameter_t *interrupt_to_configure);
/** @brief Configure which interrupt source can trigger INT2.
* @param[in] interrupt_to_configure structure with the corresponding state to INT2.
* @return 0 on success, negative value on error.
*/
int inv_imu_set_config_int2(struct inv_imu_device *s,
inv_imu_interrupt_parameter_t *interrupt_to_configure);
/** @brief Retrieve interrupts configuration.
* @param[in] interrupt_to_configure structure with the corresponding state to manage INT2.
* @return 0 on success, negative value on error.
*/
int inv_imu_get_config_int2(struct inv_imu_device *s,
inv_imu_interrupt_parameter_t *interrupt_to_configure);
/** @brief Read all registers containing data (temperature, accelerometer and gyroscope). Then it calls
* sensor_event_cb function passed in parameter of inv_imu_init function for each packet
* @return 0 on success, negative value on error.
*/
int inv_imu_get_data_from_registers(struct inv_imu_device *s);
/** @brief Read all available packets from the FIFO. For each packet function builds a
* sensor event containing packet data and validity information. Then it calls
* sensor_event_cb funtion passed in parameter of inv_imu_init function for each
* packet.
* @return number of valid packets read on success, negative value on error.
*/
int inv_imu_get_data_from_fifo(struct inv_imu_device *s);
/** @brief Converts ACCEL_CONFIG0_ODR_t or GYRO_CONFIG0_ODR_t enums to period expressed in us
* @param[in] odr_bitfield An ACCEL_CONFIG0_ODR_t or GYRO_CONFIG0_ODR_t enum
* @return The corresponding period expressed in us
*/
uint32_t inv_imu_convert_odr_bitfield_to_us(uint32_t odr_bitfield);
/** @brief Configure accel Output Data Rate
* @param[in] frequency The requested frequency.
* @sa ACCEL_CONFIG0_ODR_t
* @return 0 on success, negative value on error.
* @warning inv_device::register_cache::accel_config0_reg is modified by this function
*/
int inv_imu_set_accel_frequency(struct inv_imu_device *s,
const ACCEL_CONFIG0_ODR_t frequency);
/** @brief Configure gyro Output Data Rate
* @param[in] frequency The requested frequency.
* @sa GYRO_CONFIG0_ODR_t
* @return 0 on success, negative value on error.
* @warning inv_device::register_cache::gyro_config0_reg is modified by this function
*/
int inv_imu_set_gyro_frequency(struct inv_imu_device *s,
const GYRO_CONFIG0_ODR_t frequency);
/** @brief Set accel full scale range
* @param[in] accel_fsr_g requested full scale range.
* @sa ACCEL_CONFIG0_FS_SEL_t.
* @return 0 on success, negative value on error.
* @warning inv_device::register_cache::accel_config0_reg is modified by this function
*/
int inv_imu_set_accel_fsr(struct inv_imu_device *s,
ACCEL_CONFIG0_FS_SEL_t accel_fsr_g);
/** @brief Access accel full scale range
* @param[out] accel_fsr_g current full scale range.
* @sa ACCEL_CONFIG0_FS_SEL_t.
* @return 0 on success, negative value on error.
* @warning inv_device::register_cache::accel_config0_reg is relied upon by this function
*/
int inv_imu_get_accel_fsr(struct inv_imu_device *s,
ACCEL_CONFIG0_FS_SEL_t *accel_fsr_g);
/** @brief Set gyro full scale range
* @param[in] gyro_fsr_dps requested full scale range.
* @sa GYRO_CONFIG0_FS_SEL_t.
* @return 0 on success, negative value on error.
* @warning inv_device::register_cache::gyro_config0_reg is modified by this function
*/
int inv_imu_set_gyro_fsr(struct inv_imu_device *s,
GYRO_CONFIG0_FS_SEL_t gyro_fsr_dps);
/** @brief Access gyro full scale range
* @param[out] gyro_fsr_dps current full scale range.
* @sa GYRO_CONFIG0_FS_SEL_t.
* @return 0 on success, negative value on error.
* @warning inv_device::register_cache::gyro_config0_reg is relied upon by this function
*/
int inv_imu_get_gyro_fsr(struct inv_imu_device *s,
GYRO_CONFIG0_FS_SEL_t *gyro_fsr_dps);
/** @brief Set accel Low-Power averaging value
* @param[in] acc_avg requested averaging value
* @sa ACCEL_CONFIG1_ACCEL_FILT_AVG_t
* @return 0 on success, negative value on error.
*/
int inv_imu_set_accel_lp_avg(struct inv_imu_device *s,
ACCEL_CONFIG1_ACCEL_FILT_AVG_t acc_avg);
/** @brief Set accel Low-Noise bandwidth value
* @param[in] acc_bw requested averaging value
* @sa ACCEL_CONFIG1_ACCEL_FILT_BW_t
* @return 0 on success, negative value on error.
*/
int inv_imu_set_accel_ln_bw(struct inv_imu_device *s,
ACCEL_CONFIG1_ACCEL_FILT_BW_t acc_bw);
/** @brief Set gyro Low-Noise bandwidth value
* @param[in] gyr_bw requested averaging value
* @sa GYRO_CONFIG1_GYRO_FILT_BW_t
* @return 0 on success, negative value on error.
*/
int inv_imu_set_gyro_ln_bw(struct inv_imu_device *s,
GYRO_CONFIG1_GYRO_FILT_BW_t gyr_bw);
/** @brief Set timestamp resolution
* @param[in] timestamp_resol requested timestamp resolution
* @sa TMST_CONFIG1_RESOL_t
* @return 0 on success, negative value on error.
*/
int inv_imu_set_timestamp_resolution(struct inv_imu_device *s,
const TMST_CONFIG1_RESOL_t timestamp_resol);
/** @brief reset IMU fifo
* @return 0 on success, negative value on error.
*/
int inv_imu_reset_fifo(struct inv_imu_device *s);
/** @brief Enable 20 bits raw acc and raw gyr data in fifo.
* @return 0 on success, negative return code otherwise
*/
int inv_imu_enable_high_resolution_fifo(struct inv_imu_device *s);
/** @brief Disable 20 bits raw acc and raw gyr data in fifo.
* @return 0 on success, negative return code otherwise
*/
int inv_imu_disable_high_resolution_fifo(struct inv_imu_device *s);
/** @brief Configure Fifo
* @param[in] fifo_config Fifo configuration method :
* if FIFO is enabled, data are pushed to FIFO and FIFO THS interrupt is set
* if FIFO is disabled, data are not pused to FIFO and DRDY interrupt is set
* @sa INV_IMU_FIFO_CONFIG_t
*/
int inv_imu_configure_fifo(struct inv_imu_device *s,
INV_IMU_FIFO_CONFIG_t fifo_config);
/** @brief Get FIFO timestamp resolution
* @return the timestamp resolution in us as a q24 or 0 in case of error
*/
int32_t inv_imu_get_fifo_timestamp_resolution_us_q24(struct inv_imu_device *s);
/** @brief Get register timestamp resolution
* @return the timestamp resolution in us as a q24 or 0 in case of error
*/
uint32_t inv_imu_get_reg_timestamp_resolution_us_q24(struct inv_imu_device *s);
/** @brief Enable Wake On Motion.
* @param[in] wom_x_th threshold value for the Wake on Motion Interrupt for X-axis accelerometer.
* @param[in] wom_y_th threshold value for the Wake on Motion Interrupt for Y-axis accelerometer.
* @param[in] wom_z_th threshold value for the Wake on Motion Interrupt for Z-axis accelerometer.
* @param[in] wom_int select which mode between AND/OR is used to generate interrupt.
* @param[in] wom_dur select the number of overthreshold event to wait before generating interrupt.
* @return 0 on success, negative value on error.
*/
int inv_imu_configure_wom(struct inv_imu_device *s,
const uint8_t wom_x_th,
const uint8_t wom_y_th,
const uint8_t wom_z_th,
WOM_CONFIG_WOM_INT_MODE_t wom_int,
WOM_CONFIG_WOM_INT_DUR_t wom_dur);
/** @brief Enable Wake On Motion.
* note : WoM requests to have the accelerometer enabled to work.
* As a consequence Fifo water-mark interrupt is disabled to only trigger WoM interrupts.
* To have good performance, it's recommended to set accelerometer ODR (Output Data Rate) to 20ms
* and the accelerometer in Low Power Mode.
* @return 0 on success, negative value on error.
*/
int inv_imu_enable_wom(struct inv_imu_device *s);
/** @brief Disable Wake On Motion.
* note : Fifo water-mark interrupt is re-enabled when WoM is disabled.
* @return 0 on success, negative value on error.
*/
int inv_imu_disable_wom(struct inv_imu_device *s);
/** @brief Start DMP for APEX algorithms and selftest
* @return 0 on success, negative value on error.
*/
int inv_imu_start_dmp(struct inv_imu_device *s);
/** @brief Reset DMP for APEX algorithms and selftest
* @return 0 on success, negative value on error.
*/
int inv_imu_reset_dmp(struct inv_imu_device *s,
const APEX_CONFIG0_DMP_MEM_RESET_t sram_reset);
/** @breif Set the UI endianness and set the inv_device endianness field
* @return 0 on success, negative value on error.
*/
int inv_imu_set_endianness(struct inv_imu_device *s,
INTF_CONFIG0_DATA_ENDIAN_t endianness);
/** @breif Read the UI endianness and set the inv_device endianness field
* @return 0 on success, negative value on error.
*/
int inv_imu_get_endianness(struct inv_imu_device *s);
/** @brief Configure Fifo decimation
* @param[in] requested decimation factor value from 2 to 256
* @return 0 on success, negative value on error.
*/
int inv_imu_configure_fifo_data_rate(struct inv_imu_device *s,
FDR_CONFIG_FDR_SEL_t dec_factor);
/** @brief Return driver version x.y.z-suffix as a char array
* @retval driver version a char array "x.y.z-suffix"
*/
const char * inv_imu_get_version(void);
#ifdef __cplusplus
}
#endif
#endif /* _INV_IMU_DRIVER_H_ */
/** @} */
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/imu/inv_imu_extfunc.h
+64
View File
@@ -0,0 +1,64 @@
/*
* ________________________________________________________________________________________________________
* Copyright (c) 2017 InvenSense Inc. All rights reserved.
*
* This software, related documentation and any modifications thereto (collectively "Software") is subject
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
* and other intellectual property rights laws.
*
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
* from InvenSense is strictly prohibited.
*
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
* OF THE SOFTWARE.
* ________________________________________________________________________________________________________
*/
/** @defgroup DriverExt IMU driver extern functions
* @brief Extern functions for IMU devices
* @ingroup Driver
* @{
*/
/** @file inv_imu_extfunc.h
* Extern functions for IMU devices
*/
#ifndef _INV_IMU_EXTFUNC_H_
#define _INV_IMU_EXTFUNC_H_
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
/** @brief Hook for low-level high res system sleep() function to be implemented by upper layer
* ~100us resolution is sufficient
* @param[in] us number of us the calling thread should sleep
*/
extern void inv_imu_sleep_us(uint32_t us);
/** @brief Hook for low-level high res system get_time() function to be implemented by upper layer
* Value shall be on 64bit with a 1 us resolution
* @return The current time in us
*/
extern uint64_t inv_imu_get_time_us(void);
#ifdef __cplusplus
}
#endif
#endif /* _INV_IMU_EXTFUNC_H_ */
/** @} */
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/imu/inv_imu_regmap.h
+3391
View File
File diff suppressed because it is too large Load Diff
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/imu/inv_imu_selftest.c
+179
View File
@@ -0,0 +1,179 @@
/*
* ________________________________________________________________________________________________________
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
*
* This software, related documentation and any modifications thereto (collectively "Software") is subject
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
* and other intellectual property rights laws.
*
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
* from InvenSense is strictly prohibited.
*
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
* OF THE SOFTWARE.
* ________________________________________________________________________________________________________
*/
#include "inv_imu_selftest.h"
#include "inv_imu_extfunc.h"
#include "inv_imu_transport.h"
static int configure_selftest_parameters(struct inv_imu_device *s,
const inv_imu_selftest_parameters_t st_params);
int inv_imu_run_selftest(struct inv_imu_device *s,
const inv_imu_selftest_parameters_t st_params,
inv_imu_selftest_output_t *st_output)
{
int status = 0;
uint8_t value;
uint8_t data[2] = {0};
uint8_t st_done = 0;
int polling_timeout_ms = 1000;
/* Disables Gyro/Accel sensors */
status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &value);
value &= ~(PWR_MGMT0_ACCEL_MODE_MASK | PWR_MGMT0_GYRO_MODE_MASK);
status |= inv_imu_write_reg(s, PWR_MGMT0, 1, &value);
/* Enable RC oscillator */
status |= inv_imu_switch_on_mclk(s);
/* Clear DMP SRAM (1 ms wait included in `inv_imu_reset_dmp()`) */
status |= inv_imu_reset_dmp(s, APEX_CONFIG0_DMP_MEM_RESET_APEX_ST_EN);
/* Update `dmp_is_on` since APEX features will have to restart from scratch */
s->dmp_is_on = 0;
/* Load self-test data */
status |= inv_imu_load_selftest_data(s);
/* Set self-test parameters */
status |= configure_selftest_parameters(s, st_params);
/*
* Enable accel and/or gyro self-test.
* If both accel and gyro self-test are enabled,
* they should be set simultaneously in the same write access
*/
status |= inv_imu_read_reg(s, SELFTEST_MREG1, 1, &value);
value &= ~SELFTEST_EN;
value |= (uint8_t)st_params.st_control;
status |= inv_imu_write_reg(s, SELFTEST_MREG1, 1, &value);
/* Poll int_status_st_done bit */
do {
inv_imu_sleep_us(1000);
status |= inv_imu_read_reg(s, INT_STATUS, 1, &st_done);
st_done &= INT_STATUS_ST_INT_MASK;
if (0 == --polling_timeout_ms)
return (status | -1); /* Return error if timeout is reached */
} while ( !st_done /* Exit if ST_DONE */
&& !status /* Or if error is detected */);
/* Read self-test results */
status |= inv_imu_read_reg(s, ST_STATUS1_MREG1, 2, &data[0]);
st_output->accel_status = (data[0] & ST_STATUS1_ACCEL_ST_PASS_MASK) >> ST_STATUS1_ACCEL_ST_PASS_POS;
st_output->ax_status = (data[0] & ST_STATUS1_AX_ST_PASS_MASK) >> ST_STATUS1_AX_ST_PASS_POS;
st_output->ay_status = (data[0] & ST_STATUS1_AY_ST_PASS_MASK) >> ST_STATUS1_AY_ST_PASS_POS;
st_output->az_status = (data[0] & ST_STATUS1_AZ_ST_PASS_MASK) >> ST_STATUS1_AZ_ST_PASS_POS;
st_output->gyro_status = (data[1] & ST_STATUS2_GYRO_ST_PASS_MASK) >> ST_STATUS2_GYRO_ST_PASS_POS;
st_output->gyro_status |= ((data[1] & ST_STATUS2_ST_INCOMPLETE_MASK) >> ST_STATUS2_ST_INCOMPLETE_POS) << 1;
st_output->gx_status = (data[1] & ST_STATUS2_GX_ST_PASS_MASK) >> ST_STATUS2_GX_ST_PASS_POS;
st_output->gy_status = (data[1] & ST_STATUS2_GY_ST_PASS_MASK) >> ST_STATUS2_GY_ST_PASS_POS;
st_output->gz_status = (data[1] & ST_STATUS2_GZ_ST_PASS_MASK) >> ST_STATUS2_GZ_ST_PASS_POS;
/* Disable self-test */
status |= inv_imu_read_reg(s, SELFTEST_MREG1, 1, &value);
value &= ~SELFTEST_EN;
value |= (uint8_t)SELFTEST_DIS;
status |= inv_imu_write_reg(s, SELFTEST_MREG1, 1, &value);
/* Reset FIFO because ST data may have been pushed to it */
status |= inv_imu_reset_fifo(s);
/* Restore idle bit */
status |= inv_imu_switch_off_mclk(s);
return status;
}
int inv_imu_init_selftest_parameters_struct(struct inv_imu_device *s,
inv_imu_selftest_parameters_t *st_params)
{
(void)s;
st_params->st_num_samples = ST_CONFIG_16_SAMPLES;
st_params->st_control = (SELFTEST_ACCEL_GYRO_ST_EN_t)SELFTEST_EN;
return 0;
}
int inv_imu_load_selftest_data(struct inv_imu_device *s)
{
int status = 0;
uint8_t value;
/* Enable RC oscillator */
status |= inv_imu_switch_on_mclk(s);
/* Set up OTP controller to reload factory-trimmed self-test response into SRAM */
status |= inv_imu_read_reg(s, OTP_CONFIG_MREG1, 1, &value);
value &= ~OTP_CONFIG_OTP_COPY_MODE_MASK;
value |= (uint8_t)OTP_CONFIG_OTP_COPY_ST_DATA;
status |= inv_imu_write_reg(s, OTP_CONFIG_MREG1, 1, &value);
/* Take the OTP macro out of power-down mode */
status |= inv_imu_read_reg(s, OTP_CTRL7_MREG2, 1, &value);
value &= ~OTP_CTRL7_OTP_PWR_DOWN_MASK;
value |= (uint8_t)OTP_CTRL7_PWR_DOWN_DIS;
status |= inv_imu_write_reg(s, OTP_CTRL7_MREG2, 1, &value);
/* Wait for voltage generator to power on */
inv_imu_sleep_us(100);
/* Host should disable INT function first before kicking off OTP copy operation */
/* Trigger OTP to reload data (this time in self-test mode) */
status |= inv_imu_read_reg(s, OTP_CTRL7_MREG2, 1, &value);
value &= ~OTP_CTRL7_OTP_RELOAD_MASK;
value |= (uint8_t)OTP_CTRL7_OTP_RELOAD_EN;
status |= inv_imu_write_reg(s, OTP_CTRL7_MREG2, 1, &value);
/* Wait for OTP reload */
inv_imu_sleep_us(20);
/* Disable RC oscillator */
status |= inv_imu_switch_off_mclk(s);
return status;
}
static int configure_selftest_parameters(struct inv_imu_device *s,
const inv_imu_selftest_parameters_t st_params)
{
int status = 0;
uint8_t value;
/* Self-test configuration cannot be updated if it already running */
status |= inv_imu_read_reg(s, SELFTEST_MREG1, 1, &value);
if ((value & SELFTEST_EN) != SELFTEST_DIS)
return INV_ERROR_UNEXPECTED;
status |= inv_imu_read_reg(s, ST_CONFIG_MREG1, 1, &value);
value &= ~((uint8_t)ST_CONFIG_ST_NUMBER_SAMPLE_MASK
| (uint8_t)ST_CONFIG_ACCEL_ST_LIM_MASK
| (uint8_t)ST_CONFIG_GYRO_ST_LIM_MASK);
value |= (uint8_t)st_params.st_num_samples
| (uint8_t)ST_CONFIG_ACCEL_ST_LIM_50
| (uint8_t)ST_CONFIG_GYRO_ST_LIM_50;
status |= inv_imu_write_reg(s, ST_CONFIG_MREG1, 1, &value);
return status;
}
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/imu/inv_imu_selftest.h
+101
View File
@@ -0,0 +1,101 @@
/*
* ________________________________________________________________________________________________________
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
*
* This software, related documentation and any modifications thereto (collectively "Software") is subject
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
* and other intellectual property rights laws.
*
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
* from InvenSense is strictly prohibited.
*
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
* OF THE SOFTWARE.
* ________________________________________________________________________________________________________
*/
/** @defgroup DriverST SelfTest IMU selftest
* @brief Low-level function to run selftest on a IMU device
* @ingroup Driver
* @{
*/
/** @file inv_imu_selftest.h
* Low-level function to run selftest on a IMU device
*/
#ifndef _INV_IMU_SELFTEST_H_
#define _INV_IMU_SELFTEST_H_
#include <stdint.h>
#include "InvExport.h"
#include "inv_imu_defs.h"
#include "inv_imu_driver.h"
#ifdef __cplusplus
extern "C" {
#endif
/* forward declaration */
struct inv_imu_device;
/** @brief Self-test input parameters
*/
typedef struct {
ST_CONFIG_NUM_SAMPLES_t st_num_samples; /**< Number of samples used to perform self-test */
SELFTEST_ACCEL_GYRO_ST_EN_t st_control; /**< Define which sensor is under self-test */
} inv_imu_selftest_parameters_t;
/** @brief Self-test routine outputs
*/
typedef struct {
int8_t accel_status; /**< global accelerometer self-test passed */
int8_t gyro_status; /**< global gyroscope self-test status: st_pass (bit0), st_incomplete (bit1) */
int8_t ax_status; /**< AX self-test status */
int8_t ay_status; /**< AY self-test status */
int8_t az_status; /**< AZ self-test status */
int8_t gx_status; /**< GX self-test status */
int8_t gy_status; /**< GY self-test status */
int8_t gz_status; /**< GZ self-test status */
} inv_imu_selftest_output_t;
/**
* @brief Perform hardware self-test for Accel and Gyro
* @param[in] Self-test parameters (see inv_imu_selftest_parameters_t)
* @param[out] Self-test results (see inv_imu_selftest_output_t)
* @return 0 on completion, negative number if intermediate errors occurred
*/
int inv_imu_run_selftest(struct inv_imu_device *s,
const inv_imu_selftest_parameters_t st_params,
inv_imu_selftest_output_t *st_output);
/** @brief Fill the self-test configuration structure with default configuration
* @param[in] selftest_params self-test parameters to be initialized
* @return 0 on success, negative return code otherwise
*/
int inv_imu_init_selftest_parameters_struct(struct inv_imu_device *s,
inv_imu_selftest_parameters_t *selftest_params);
/** @brief Load self-test data
* @return 0 on success, negative return code otherwise
*/
int inv_imu_load_selftest_data(struct inv_imu_device *s);
#ifdef __cplusplus
}
#endif
#endif /* _INV_IMU_SELFTEST_H_ */
/** @} */
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/imu/inv_imu_transport.c
+257
View File
@@ -0,0 +1,257 @@
/*
* ________________________________________________________________________________________________________
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
*
* This software, related documentation and any modifications thereto (collectively "Software") is subject
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
* and other intellectual property rights laws.
*
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
* from InvenSense is strictly prohibited.
*
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
* OF THE SOFTWARE.
* ________________________________________________________________________________________________________
*/
#include "inv_imu_extfunc.h"
#include "inv_imu_transport.h"
#include "inv_imu_regmap.h"
#include "InvError.h"
/* Function definition */
static uint8_t *get_register_cache_addr(struct inv_imu_device *s, uint32_t reg);
static int write_sreg(struct inv_imu_device *s, uint8_t reg, uint32_t len, const uint8_t *buf);
static int read_sreg(struct inv_imu_device *s, uint8_t reg, uint32_t len, uint8_t *buf);
static int write_mclk_reg(struct inv_imu_device *s, uint16_t regaddr, uint8_t wr_cnt, const uint8_t *buf);
static int read_mclk_reg(struct inv_imu_device *s, uint16_t regaddr, uint8_t rd_cnt, uint8_t *buf);
int inv_imu_init_transport(struct inv_imu_device *s)
{
int status = 0;
struct inv_imu_transport *t = (struct inv_imu_transport *)s;
status |= read_sreg(s, (uint8_t)PWR_MGMT0, 1, &(t->register_cache.pwr_mgmt0_reg));
status |= read_sreg(s, (uint8_t)GYRO_CONFIG0, 1, &(t->register_cache.gyro_config0_reg));
status |= read_sreg(s, (uint8_t)ACCEL_CONFIG0, 1, &(t->register_cache.accel_config0_reg));
status |= read_mclk_reg(s, (TMST_CONFIG1_MREG1 & 0xFFFF), 1, &(t->register_cache.tmst_config1_reg));
t->need_mclk_cnt = 0;
return status;
}
int inv_imu_read_reg(struct inv_imu_device *s, uint32_t reg, uint32_t len, uint8_t *buf)
{
uint32_t i;
int rc = 0;
for (i = 0; i < len; i++) {
uint8_t *cache_addr = get_register_cache_addr(s, reg + i);
if (cache_addr) {
buf[i] = *cache_addr;
} else {
if (!(reg & 0x10000)) {
rc |= read_mclk_reg(s, ((reg + i) & 0xFFFF), 1, &buf[i]);
} else {
rc |= read_sreg(s, (uint8_t)reg + i, len - i, &buf[i]);
break;
}
}
}
return rc;
}
int inv_imu_write_reg(struct inv_imu_device *s, uint32_t reg, uint32_t len, const uint8_t *buf)
{
uint32_t i;
int rc = 0;
for (i = 0; i < len; i++) {
uint8_t *cache_addr = get_register_cache_addr(s, reg + i);
if (cache_addr)
*cache_addr = buf[i];
if (!(reg & 0x10000))
rc |= write_mclk_reg(s, ((reg + i) & 0xFFFF), 1, &buf[i]);
}
if (reg & 0x10000)
rc |= write_sreg(s, (uint8_t)reg, len, buf);
return rc;
}
int inv_imu_switch_on_mclk(struct inv_imu_device *s)
{
int status = 0;
uint8_t data;
struct inv_imu_transport *t = (struct inv_imu_transport *)s;
/* set IDLE bit only if it is not set yet */
if (t->need_mclk_cnt == 0) {
status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &data);
data |= PWR_MGMT0_IDLE_MASK;
status |= inv_imu_write_reg(s, PWR_MGMT0, 1, &data);
if (status)
return status;
/* Check if MCLK is ready */
do {
status = inv_imu_read_reg(s, MCLK_RDY, 1, &data);
} while ((status != 0) || !(data & MCLK_RDY_MCLK_RDY_MASK));
} else {
/* Make sure it is already on */
status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &data);
if (0 == (data &= PWR_MGMT0_IDLE_MASK))
status |= INV_ERROR;
}
/* Increment the counter to keep track of number of MCLK requesters */
t->need_mclk_cnt++;
return status;
}
int inv_imu_switch_off_mclk(struct inv_imu_device *s)
{
int status = 0;
uint8_t data;
struct inv_imu_transport *t = (struct inv_imu_transport *)s;
/* Reset the IDLE but only if there is one requester left */
if (t->need_mclk_cnt == 1) {
status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &data);
data &= ~PWR_MGMT0_IDLE_MASK;
status |= inv_imu_write_reg(s, PWR_MGMT0, 1, &data);
} else {
/* Make sure it is still on */
status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &data);
if (0 == (data &= PWR_MGMT0_IDLE_MASK))
status |= INV_ERROR;
}
/* Decrement the counter */
t->need_mclk_cnt--;
return status;
}
/* Static function */
static uint8_t *get_register_cache_addr(struct inv_imu_device *s, uint32_t reg)
{
struct inv_imu_transport *t = (struct inv_imu_transport *)s;
switch(reg) {
case PWR_MGMT0: return &(t->register_cache.pwr_mgmt0_reg);
case GYRO_CONFIG0: return &(t->register_cache.gyro_config0_reg);
case ACCEL_CONFIG0: return &(t->register_cache.accel_config0_reg);
case TMST_CONFIG1_MREG1: return &(t->register_cache.tmst_config1_reg);
default: return (uint8_t *)0; // Not found
}
}
static int read_sreg(struct inv_imu_device *s, uint8_t reg, uint32_t len, uint8_t *buf)
{
struct inv_imu_serif *serif = (struct inv_imu_serif *)s;
if (len > serif->max_read)
return INV_ERROR_SIZE;
if (serif->read_reg(serif, reg, buf, len) != 0)
return INV_ERROR_TRANSPORT;
return 0;
}
static int write_sreg(struct inv_imu_device *s, uint8_t reg, uint32_t len, const uint8_t *buf)
{
struct inv_imu_serif *serif = (struct inv_imu_serif *)s;
if (len > serif->max_write)
return INV_ERROR_SIZE;
if (serif->write_reg(serif, reg, buf, len) != 0)
return INV_ERROR_TRANSPORT;
return 0;
}
static int read_mclk_reg(struct inv_imu_device *s, uint16_t regaddr, uint8_t rd_cnt, uint8_t *buf)
{
uint8_t data;
uint8_t blk_sel = (regaddr & 0xFF00) >> 8;
int status = 0;
// Have IMU not in IDLE mode to access MCLK domain
status |= inv_imu_switch_on_mclk(s);
// optimize by changing BLK_SEL only if not NULL
if (blk_sel)
status |= write_sreg(s, (uint8_t)BLK_SEL_R & 0xff, 1, &blk_sel);
data = (regaddr & 0x00FF);
status |= write_sreg(s, (uint8_t)MADDR_R, 1, &data);
inv_imu_sleep_us(10);
status |= read_sreg(s, (uint8_t)M_R, rd_cnt, buf);
inv_imu_sleep_us(10);
if (blk_sel) {
data = 0;
status |= write_sreg(s, (uint8_t)BLK_SEL_R, 1, &data);
}
// switch OFF MCLK if needed
status |= inv_imu_switch_off_mclk(s);
return status;
}
static int write_mclk_reg(struct inv_imu_device *s, uint16_t regaddr, uint8_t wr_cnt, const uint8_t *buf)
{
uint8_t data;
uint8_t blk_sel = (regaddr & 0xFF00) >> 8;
int status = 0;
// Have IMU not in IDLE mode to access MCLK domain
status |= inv_imu_switch_on_mclk(s);
// optimize by changing BLK_SEL only if not NULL
if (blk_sel)
status |= write_sreg(s, (uint8_t)BLK_SEL_W, 1, &blk_sel);
data = (regaddr & 0x00FF);
status |= write_sreg(s, (uint8_t)MADDR_W, 1, &data);
for (uint8_t i = 0; i < wr_cnt; i++) {
status |= write_sreg(s, (uint8_t)M_W, 1, &buf[i]);
inv_imu_sleep_us(10);
}
if (blk_sel) {
data = 0;
status = write_sreg(s, (uint8_t)BLK_SEL_W, 1, &data);
}
status |= inv_imu_switch_off_mclk(s);
return status;
}
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/imu/inv_imu_transport.h
+122
View File
@@ -0,0 +1,122 @@
/*
* ________________________________________________________________________________________________________
* Copyright (c) 2015-2015 InvenSense Inc. All rights reserved.
*
* This software, related documentation and any modifications thereto (collectively "Software") is subject
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
* and other intellectual property rights laws.
*
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
* from InvenSense is strictly prohibited.
*
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
* OF THE SOFTWARE.
* ________________________________________________________________________________________________________
*/
/** @defgroup Transport IMU transport
* @brief Low-level IMU SCLK register access
* @ingroup Driver
* @{
*/
/** @file inv_imu_transport.h
* Low-level IMU SCLK register access
*/
#ifndef _INV_IMU_TRANSPORT_H_
#define _INV_IMU_TRANSPORT_H_
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
/* forward declaration */
struct inv_imu_device;
/** @brief enumeration of serial interfaces available on IMU */
typedef enum
{
UI_I2C,
UI_SPI4,
UI_SPI3
} SERIAL_IF_TYPE_t;
/** @brief basesensor serial interface
*/
struct inv_imu_serif {
void *context;
int (*read_reg)(struct inv_imu_serif *serif, uint8_t reg, uint8_t *buf, uint32_t len);
int (*write_reg)(struct inv_imu_serif *serif, uint8_t reg, const uint8_t *buf, uint32_t len);
int (*configure)(struct inv_imu_serif *serif);
uint32_t max_read;
uint32_t max_write;
SERIAL_IF_TYPE_t serif_type;
};
/** @brief transport interface
*/
struct inv_imu_transport {
struct inv_imu_serif serif; /**< Warning : this field MUST be the first one of struct inv_imu_transport */
/** @brief Contains mirrored values of some IP registers */
struct register_cache {
uint8_t pwr_mgmt0_reg; /**< PWR_MGMT0, Bank: 0 */
uint8_t gyro_config0_reg; /**< GYRO_CONFIG0, Bank: 0 */
uint8_t accel_config0_reg; /**< ACCEL_CONFIG0, Bank: 0 */
uint8_t tmst_config1_reg; /**< TMST_CONFIG1, Bank: MREG_TOP1 */
} register_cache; /**< Store mostly used register values on SRAM */
uint8_t need_mclk_cnt; /**< internal counter to keep track of everyone that needs MCLK */
};
/** @brief Init cache variable.
* @return 0 in case of success, -1 for any error
*/
int inv_imu_init_transport(struct inv_imu_device *s);
/** @brief Reads data from a register on IMU.
* @param[in] reg register address to be read
* @param[in] len number of byte to be read
* @param[out] buf output data from the register
* @return 0 in case of success, -1 for any error
*/
int inv_imu_read_reg(struct inv_imu_device *s, uint32_t reg, uint32_t len, uint8_t *buf);
/** @brief Writes data to a register on IMU.
* @param[in] reg register address to be written
* @param[in] len number of byte to be written
* @param[in] buf input data to write
* @return 0 in case of success, -1 for any error
*/
int inv_imu_write_reg(struct inv_imu_device *s, uint32_t reg, uint32_t len, const uint8_t *buf);
/** @brief Enable MCLK so that MREG are clocked and system beyond SOI can be safely accessed
* @return 0 in case of success, -1 for any error
*/
int inv_imu_switch_on_mclk(struct inv_imu_device *s);
/** @brief Disable MCLK so that MREG are not clocked anymore, hence reducing power consumption
* @return 0 in case of success, -1 for any error
*/
int inv_imu_switch_off_mclk(struct inv_imu_device *s);
#ifdef __cplusplus
}
#endif
#endif /* _INV_IMU_TRANSPORT_H_ */
/** @} */
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/imu/inv_imu_version.h
+37
View File
@@ -0,0 +1,37 @@
/*
* ________________________________________________________________________________________________________
* Copyright (c) 2019 InvenSense Inc. All rights reserved.
*
* This software, related documentation and any modifications thereto (collectively “Software”) is subject
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
* and other intellectual property rights laws.
*
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
* and any use, reproduction, disclosure or distribution of the Software without an express license agreement
* from InvenSense is strictly prohibited.
*
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
* PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
* TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
* EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
* INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
* DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
* NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
* OF THE SOFTWARE.
* ________________________________________________________________________________________________________
*/
#ifndef _INV_IMU_VERSION_H_
#define _INV_IMU_VERSION_H_
#ifdef __cplusplus
extern "C" {
#endif
#define INV_IMU_VERSION_STRING "2.0.4"
#ifdef __cplusplus
}
#endif
#endif /* _INV_IMU_VERSION_H_ */
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/lib_agm/invn/common/invn_types.h
+153
View File
@@ -0,0 +1,153 @@
//-----------------------------------------------------------------------------
/*
Copyright © 2014-2015 InvenSense Inc. Portions Copyright © 2014-2015 Movea. All rights reserved.
This software, related documentation and any modifications thereto (collectively “Software”) is subject
to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
and other intellectual property rights laws.
InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
and any use, reproduction, disclosure or distribution of the Software without an express license agreement
from InvenSense is strictly prohibited.
*/
//-----------------------------------------------------------------------------
#ifndef INVN_COMMON_INVN_TYPES_H_
#define INVN_COMMON_INVN_TYPES_H_
/**
* @defgroup invn_types Types
* @brief Motion Library - Type definitions.
* \details Definition of codes and error codes used within the MPL and
* returned to the user.
* Every function tries to return a meaningful error code basing
* on the occuring error condition. The error code is numeric.
*
* The available error codes and their associated values are:
* - (0) INV_SUCCESS
* - (32) INV_ERROR
* - (22 / EINVAL) INV_ERROR_INVALID_PARAMETER
* - (1 / EPERM) INV_ERROR_FEATURE_NOT_ENABLED
* - (36) INV_ERROR_FEATURE_NOT_IMPLEMENTED
* - (64) INV_ERROR_FIFO_READ_COUNT
* \todo Clean up the details documentation in order to use only the \\def one.
* \todo Add documentation to all the definitions
* \ingroup Common
* @file invn_types.h
*/
//=======================================//
//========= Integer Definition =========//
//=======================================//
#ifdef _MSC_VER
# include "inttypes.h"
#else
# include <stdint.h>
#endif
//=======================================//
//======= Fixed Point Conversion =======//
//=======================================//
//! \def INVN_FLT_TO_FXP
//! Convert the \a value from float to QN value. \ingroup invn_macro
#define INVN_FLT_TO_FXP(value, shift) ( (int32_t) ((float)(value)*(1ULL << (shift)) + ( (value>=0)-0.5f )) )
//! \def INVN_DBL_TO_FXP
//! Convert the \a value from double to QN value. \ingroup invn_macro
#define INVN_DBL_TO_FXP(value, shift) ( (int32_t) ((double)(value)*(1ULL << (shift)) + ( (value>=0)-0.5 )) )
//! \def INVN_FLT_TO_UFXP
//! Convert the \a value from float to unsigned QN value. \ingroup invn_macro
#define INVN_FLT_TO_UFXP(value, shift) ( (uint32_t) ((float)(value)*(1ULL << (shift)) + 0.5f) )
//! \def INVN_DBL_TO_UFXP
//! Convert the \a value from double to unsigned QN value. \ingroup invn_macro
#define INVN_DBL_TO_UFXP(value, shift) ( (uint32_t) ((double)(value)*(1ULL << (shift)) + 0.5) )
//! \def INVN_FXP_TO_FLT
//! Convert the \a value from QN value to float. \ingroup invn_macro
#define INVN_FXP_TO_FLT(value, shift) ( (float) (int32_t)(value) / (float)(1ULL << (shift)) )
//! \def INVN_FXP_TO_DBL
//! Convert the \a value from QN value to double. \ingroup invn_macro
#define INVN_FXP_TO_DBL(value, shift) ( (double) (int32_t)(value) / (double)(1ULL << (shift)) )
//! \def INVN_UFXP_TO_FLT
//! Convert the \a value from unsigned QN value to float. \ingroup invn_macro
#define INVN_UFXP_TO_FLT(value, shift) ( (float) (uint32_t)(value) / (float)(1ULL << (shift)) )
//! \def INVN_UFXP_TO_DBL
//! Convert the \a value from unsigned QN value to double. \ingroup invn_macro
#define INVN_UFXP_TO_DBL(value, shift) ( (double) (uint32_t)(value) / (double)(1ULL << (shift)) )
//! \def INVN_CONVERT_FLT_TO_FXP
//! Macro to convert float values from an address into QN values, and copy them to another address. \ingroup invn_macro
#define INVN_CONVERT_FLT_TO_FXP(fltptr, fixptr, length, shift) { int32_t i; for(i=0; i<(length); ++i) (fixptr)[i] = INVN_FLT_TO_FXP((fltptr)[i], shift); }
//! \def INVN_CONVERT_FLT_TO_UFXP
//! Macro to convert float values from an address into unsigned QN values, and copy them to another address. \ingroup invn_macro
#define INVN_CONVERT_FLT_TO_UFXP(fltptr, fixptr, length, shift) { int32_t i; for(i=0; i<(length); ++i) (fixptr)[i] = INVN_FLT_TO_UFXP((fltptr)[i], shift); }
//! \def INVN_CONVERT_DBL_TO_FXP
//! Macro to convert double values from an address into QN values, and copy them to another address. \ingroup invn_macro
#define INVN_CONVERT_DBL_TO_FXP(fltptr, fixptr, length, shift) { int32_t i; for(i=0; i<(length); ++i) (fixptr)[i] = INVN_DBL_TO_FXP((fltptr)[i], shift); }
//! \def INVN_CONVERT_DBL_TO_UFXP
//! Macro to convert double values from an address into unsigned QN values, and copy them to another address. \ingroup invn_macro
#define INVN_CONVERT_DBL_TO_UFXP(fltptr, fixptr, length, shift) { int32_t i; for(i=0; i<(length); ++i) (fixptr)[i] = INVN_DBL_TO_UFXP((fltptr)[i], shift); }
//! \def INVN_CONVERT_FXP_TO_FLT
//! Macro to convert QN values from an address into float values, and copy them to another address. \ingroup invn_macro
#define INVN_CONVERT_FXP_TO_FLT(fixptr, fltptr, length, shift) { int32_t i; for(i=0; i<(length); ++i) (fltptr)[i] = INVN_FXP_TO_FLT((fixptr)[i], shift); }
//! \def INVN_CONVERT_UFXP_TO_FLT
//! Macro to convert unsigned QN values from an address into float values, and copy them to another address. \ingroup invn_macro
#define INVN_CONVERT_UFXP_TO_FLT(fixptr, fltptr, length, shift) { int32_t i; for(i=0; i<(length); ++i) (fltptr)[i] = INVN_UFXP_TO_FLT((fixptr)[i], shift); }
//! \def INVN_CONVERT_FXP_TO_DBL
//! Macro to convert QN values from an address into double values, and copy them to another address. \ingroup invn_macro
#define INVN_CONVERT_FXP_TO_DBL(fixptr, fltptr, length, shift) { int32_t i; for(i=0; i<(length); ++i) (fltptr)[i] = INVN_FXP_TO_DBL((fixptr)[i], shift); }
//! \def INVN_CONVERT_UFXP_TO_DBL
//! \brief Macro to convert unsigned QN values from an address into double values, and copy them to another address. \ingroup invn_macro
#define INVN_CONVERT_UFXP_TO_DBL(fixptr, fltptr, length, shift) { int32_t i; for(i=0; i<(length); ++i) (fltptr)[i] = INVN_UFXP_TO_DBL((fixptr)[i], shift); }
//=====================================//
//========= Error Definition =========//
//=====================================//
#ifndef REMOVE_INV_ERROR_T
typedef int32_t inv_error_t; /*!< Type used for error definitions. \ingroup invn_types */
#endif
//typedef int32_t mpu_error_t;
typedef int64_t mpu_time_t; /*!< Type used for mpu time. \ingroup invn_types */
// Typically I2C addresses are 8-bit, but some specifications allow for a 10-bit address
// This definition allows the length to be optimally defined for the platform
typedef uint8_t inv_i2c_addr_t; /*!< Type used for I2C addresses. \ingroup invn_types */
#ifdef __IAR_SYSTEMS_ICC__
// These are defined in standard C errno.h
#define EINVAL (22)
#define EPERM (1)
#define ENOMEM (12)
#else
#include "errno.h"
#endif
#define INVN_SUCCESS (0) /*!< Constant definition for success. \ingroup invn_types */
#define INVN_ERROR_BASE (0x20) /*!< Constant definition for basic error. Value is \b 32 \ingroup invn_types */
#define INVN_ERROR (INVN_ERROR_BASE) /*!< Constant definition for error. Value is \b 32 \ingroup invn_types */
#define INVN_ERROR_FEATURE_NOT_ENABLED (EPERM) /*!< Constant definition for feature not enabled error. \ingroup invn_types */
#define INVN_ERROR_FEATURE_NOT_IMPLEMENTED (INVN_ERROR_BASE + 4) /*!< Constant definition for feature not implemented error. \ingroup invn_types */
#define INVN_ERROR_INVALID_PARAMETER (EINVAL) /*!< Constant definition for invalid parameter error. \ingroup invn_types */
#define INVN_ERROR_FILE_OPEN (INVN_ERROR_BASE + 14) /*!< Constant definition for opening file error. \ingroup invn_types */
#define INVN_ERROR_FILE_READ (INVN_ERROR_BASE + 15) /*!< Constant definition for reading file error. \ingroup invn_types */
#define INVN_ERROR_FILE_WRITE (INVN_ERROR_BASE + 16) /*!< Constant definition for writing file error. \ingroup invn_types */
#define INVN_ERROR_INVALID_CONFIGURATION (INVN_ERROR_BASE + 17) /*!< Constant definition for invalid configuration error. \ingroup invn_types */
/* Serial Communication */
#define INVN_ERROR_SERIAL_OPEN_ERROR (INVN_ERROR_BASE + 21) /*!< Constant definition for serial open error. \ingroup invn_types */
#define INVN_ERROR_SERIAL_READ (INVN_ERROR_BASE + 22) /*!< Constant definition for serial read error. \ingroup invn_types */
#define INVN_ERROR_SERIAL_WRITE (INVN_ERROR_BASE + 23) /*!< Constant definition for serial write error. \ingroup invn_types */
/* Fifo */
#define INVN_ERROR_FIFO_OVERFLOW (INVN_ERROR_BASE + 30) /*!< Constant definition for fifo overflow error. \ingroup invn_types */
#define INVN_ERROR_FIFO_FOOTER (INVN_ERROR_BASE + 31) /*!< Constant definition for fifo footer error. \ingroup invn_types */
#define INVN_ERROR_FIFO_READ_COUNT (INVN_ERROR_BASE + 32) /*!< Constant definition for fifo read count error. \ingroup invn_types */
#define INVN_ERROR_FIFO_READ_DATA (INVN_ERROR_BASE + 33) /*!< Constant definition for fifo read data error. \ingroup invn_types */
/* OS interface errors */
#define INVN_ERROR_OS_BAD_HANDLE (INVN_ERROR_BASE + 61) /*!< Constant definition for OS bad handle error. \ingroup invn_types */
#define INVN_ERROR_OS_CREATE_FAILED (INVN_ERROR_BASE + 62) /*!< Constant definition for OS create failed error. \ingroup invn_types */
#define INVN_ERROR_OS_LOCK_FAILED (INVN_ERROR_BASE + 63) /*!< Constant definition for OS lock failed error. \ingroup invn_types */
/* Warning */
#define INVN_WARNING_SEMAPHORE_TIMEOUT (INVN_ERROR_BASE + 86) /*!< Constant definition for semaphore timeout warning. \ingroup invn_types */
#endif // INVN_COMMON_INVN_TYPES_H_
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/lib_agm/invn_algo_agm.h
+156
View File
@@ -0,0 +1,156 @@
/*
$License:
Copyright (C) 2018 InvenSense Corporation, All Rights Reserved.
$
*/
#ifndef _INVN_ALGO_AGM_H_
#define _INVN_ALGO_AGM_H_
#include "invn_types.h"
#ifdef __cplusplus
extern "C" {
#endif
/** \defgroup AGM AGM
* \brief Algorithm that provides device orientation. Algorithm inputs are raw Accelerometer, Gyroscope and Magnetometer data.
* Algorithm outputs: calibrated sensor and 9-axis sensor fusion.
* \warning supported sampling frequency [50 Hz-1000 Hz]
* \warning supported gyroscope FSR [250 dps, 500 dps, 1000 dps, 2000 dps, 4000 dps]
* \warning supported accelerometer FSR [1 g, 2 g, 4 g, 8 g, 16 g]
*/
#define INVN_ALGO_AGM_INPUT_MASK_ACC 1 ///< Raw Accel update mask
#define INVN_ALGO_AGM_INPUT_MASK_GYR 2 ///< Raw Gyro update mask
#define INVN_ALGO_AGM_INPUT_MASK_MAG 4 ///< Raw Mag update mask
#define INVN_ALGO_AGM_OUTPUT_MASK_ACCEL_CAL 1 ///< Accel cal output update mask
#define INVN_ALGO_AGM_OUTPUT_MASK_GYRO_CAL 2 ///< Gyro cal output update mask
#define INVN_ALGO_AGM_OUTPUT_MASK_MAG_CAL 4 ///< Mag cal output update mask
#define INVN_ALGO_AGM_OUTPUT_MASK_QUAT_AG 8 ///< Game Rotation Vector (Accel and Gyro Fusion) output update mask
#define INVN_ALGO_AGM_OUTPUT_MASK_QUAT_AGM 16 ///< Rotation Vector (Accel, Gyro and Magnetometer Fusion) output update mask
#define INVN_ALGO_AGM_OUTPUT_MASK_GRAVITY 32 ///< Gravity vector output update mask
#define INVN_ALGO_AGM_OUTPUT_MASK_LINEARACC 64 ///< Linear acceleration vector output update mask
/* Forward declarations */
struct inv_icm426xx;
/*! \struct InvnAlgoAGMInput
* AGM input structure (raw data) \ingroup AGM
*/
typedef struct
{
int32_t mask; /*!< mask to specify updated inputs. */
int64_t sRimu_time_us; /*!< timestamp \f$ [\mu s]\f$ of raw accel and gyro */
int32_t sRacc_data[3]; /*!< raw accelerometer in high resolution mode. Expect Full Scale Value coded on 20 bit (i.e. +/- FSR g = 1<<19 LSB) */
int32_t sRgyr_data[3]; /*!< raw gyroscope in high resolution mode. Expect Full Scale Value coded on 20 bit (i.e. +/- FSR dps = 1<<19 LSB) */
int16_t sRtemp_data; /*!< raw temperature */
int64_t sRmag_time_us; /*!< timestamp of raw mag */
int32_t sRmag_data[3]; /*!< raw mag */
} InvnAlgoAGMInput;
/*! \struct InvnAlgoAGMOutput
* AGM output structure (calibrated sensors and fusion output) \ingroup AGM
*/
typedef struct
{
int32_t mask; /*!< mask to specify updated outputs */
int32_t acc_uncal_q16[3]; /*!< uncalibrated accelerometer (1 g = 1<<16) */
int32_t acc_cal_q16[3]; /*!< calibrated accelerometer (1 g = 1<<16) */
int32_t acc_bias_q16[3]; /*!< accelerometer bias (1 g = 1<<16)*/
int8_t acc_accuracy_flag; /*!< accelerometer accuracy from 0(non calibrated) to 3(well calibrated) */
int32_t gyr_uncal_q16[3]; /*!< uncalibrated gyroscope (1 dps = 1<<16) */
int32_t gyr_cal_q16[3]; /*!< calibrated gyroscope (1 dps = 1<<16) */
int32_t gyr_bias_q16[3]; /*!< gyro bias (1 dps = 1<<16)*/
int8_t gyr_accuracy_flag; /*!< gyro accuracy, from 0(non calibrated) to 3(well calibrated) */
int32_t mag_uncal_q16[3]; /*!< uncalibrated magnetometer (1uT = 1<<16) */
int32_t mag_cal_q16[3]; /*!< calibrated magnetometer (1uT = 1<<16) */
int32_t mag_bias_q16[3]; /*!< magnetometer bias (1uT = 1<<16) */
int8_t mag_accuracy_flag; /*!< magnetometer accuracy, from 0(non calibrated) to 3(well calibrated) */
int32_t grv_quat_q30[4]; /*!< 6-axis (accel and gyro fusion) quaternion */
int32_t rv_quat_q30[4]; /*!< 9-axis (accel, gyro and magnetometer fusion) quaternion */
int32_t rv_accuracy_q27; /*!< 9-axis (accel, gyro and magnetometer fusion) 3\sigma accuracy in rad */
int32_t gravity_q16[3]; /*!< gravity estimation in sensor frame */
int32_t linear_acc_q16[3]; /*!< linear acceleration estimation in sensor frame */
int32_t temp_degC_q16; /*!< temperature (1 \f$ [^{\circ}C]\f$ = 1<<16)*/
} InvnAlgoAGMOutput;
/*! \struct InvnAlgoAGMConfig
* AGM configuration structure (sensor related settings) \ingroup AGM
*/
typedef struct
{
int32_t * acc_bias_q16; /*!< Previously stored accel bias pointer. If pointer is NULL or 0, offset will be set to { 0, 0, 0} */
int32_t * gyr_bias_q16; /*!< Previously stored gyro bias pointer. If pointer is NULL or 0, offset will be set to { 0, 0, 0} */
int32_t * mag_bias_q16; /*!< mag_bias_q16 Previously stored mag bias pointer If pointer is NULL or 0, offset will be set to { 0, 0, 0} */
int8_t acc_accuracy; /*!< Previously stored accelerometer bias accuracy (0 to 3) */
int8_t gyr_accuracy; /*!< Previously stored gyroscope bias accuracy (0 to 3) */
int8_t mag_accuracy; /*!< Previously stored magnetometer bias accuracy (0 to 3) */
int32_t acc_fsr; /*!< accelerometer full scale range [g] */
int32_t gyr_fsr; /*!< gyroscope full scale range [dps] */
uint32_t acc_odr_us; /*!< accelerometer output data rate in \f$ [\mu s]\f$ */
uint32_t gyr_odr_us; /*!< gyroscope output data rate \f$ [\mu s]\f$ */
int32_t mag_sc_q16; /*!< magnetometer sensitivity (uT/LSB, e.g. mag_uT = (mag_sc_q16 * raw_mag_LSB)/65536) */
uint32_t mag_odr_us; /*!< magnetometer output data rate \f$ [\mu s]\f$ */
int32_t temp_sensitivity; /*!< temperature sensitivity in q30 (if temperature(\f$ ^{\circ}C \f$) = LSB * k + z, then temp_sensitivity = k) */
int32_t temp_offset; /*!< temperature offset in q16 (if temperature(\f$ ^{\circ}C \f$) = LSB * k + z, then temp_offset = z) */
} InvnAlgoAGMConfig;
/*!
* \brief Return library version x.y.z-suffix as a char array
* \retval library version a char array "x.y.z-suffix"
* \ingroup AGM
*/
const char * invn_algo_agm_version(void);
/*!
* \brief Initializes algorithms with default parameters and reset states.
* (\icm_device[in] Invensense ICM426XX device pointer. Only when gyro assisted is enabled.)
* \config[in] algo init parameters structure.
* \return initialization success indicator.
* \retval 0 Success
* \retval 1 Fail
* \ingroup AGM
*/
#ifdef WITH_GYRO_ASSIST
uint8_t invn_algo_agm_init_a(struct inv_icm426xx * icm_device, const InvnAlgoAGMConfig * config);
#else
uint8_t invn_algo_agm_init(const InvnAlgoAGMConfig * config);
#endif
/*!
* \brief Sets algo config structure.
* \config[in] config structure of the algo.
* \ingroup AGM
*/
void invn_algo_agm_set_config(const InvnAlgoAGMConfig * config);
/*!
* \brief Performs algorithm computation.
* \in inputs algorithm input. Input mask (inputs->mask) should be set with respect to new sensor data in InvnAlgoAGMInput.
* \out outputs algorithm output. Output mask (outputs->mask) reports updated outputs in InvnAlgoAGMOutput.
* \ingroup AGM
*/
void invn_algo_agm_process(const InvnAlgoAGMInput *inputs, InvnAlgoAGMOutput *outputs);
#ifdef __cplusplus
}
#endif
#endif
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/lib_agm/libInvnAlgoAGM.a
BIN
View File
Binary file not shown.
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/Invn/lib_agm/test_vector/out_invn_agm_1.h
+3012
View File
File diff suppressed because it is too large Load Diff
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.c
+582
View File
@@ -0,0 +1,582 @@
/*******************************************************************************
* @file app_raw.c
* @author CandyPops Co.
* @version V1.0.0
* @date 2022-09-05
* @brief
******************************************************************************/
/*******************************************************************************
* [Module overview] ICM42670P IMU driver application layer
*
* Application layer module responsible for initialization, configuration,
* and data reading of the ICM42670P IMU sensor. Wraps InvenSense driver API.
*
* Key functions:
* 1) setup_imu_device() - IMU init and WHOAMI verification (0x67 = ICM42670P)
* 2) configure_imu_device() - Sensor parameter configuration
* - Accelerometer: +/-4g FSR, 100Hz (low-power) or 800Hz (low-noise)
* - Gyroscope: +/-2000dps FSR, 100Hz or 800Hz
* - FIFO disabled (direct register read mode)
* 3) get_imu_data() - Read sensor data from FIFO or registers
* 4) imu_callback() - Sensor data receive callback
* - Applies mounting matrix (board orientation correction)
* - info4 mode: stores data in info_imu[6]
* - BLE mode: sends 6-axis data via BLE with "rsp:" tag
* - UART mode: outputs text format to serial
* 5) imu_read_direct() - Direct I2C register read bypassing driver API
* - Configure sensor -> power ON -> wait 80ms -> read 12 bytes -> sleep
*
* Mounting matrix:
* 3x3 rotation matrix in Q30 fixed-point format, correcting the sensor's
* physical mounting orientation to match the software coordinate system.
******************************************************************************/
#include "sdk_config.h"
#include "app_raw.h"
#include "inv_imu_extfunc.h"
#include "inv_imu_driver.h"
#include "ble_nus.h"
#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"
#include "app_util_platform.h"
#include "main.h"
#include "debug_print.h"
#include "nrf_delay.h"
/*
* Data output format selection
* 0 : Raw data output (raw accel, gyro, temp)
* 1 : Scaled data output (g, dps, Celsius)
*/
#define SCALED_DATA_G_DPS 0
/* --------------------------------------------------------------------------------------
* Static and extern variables
* -------------------------------------------------------------------------------------- */
/* IMU driver object — always passed to driver API calls */
static struct inv_imu_device icm_driver;
/* Binary buffer for BLE transmission */
uint8_t imu_bin_buffer[BLE_NUS_MAX_DATA_LEN] = {0};
/*
* ICM42670P mounting matrix (Q30 fixed-point)
*
* Coordinate transform based on the sensor's physical mounting orientation.
* Q30 format: 1.0 = (1 << 30) = 0x40000000
*
* SM_REVB_DB (dev board): X->-Y, Y->X transform (90-degree rotation)
* Default (SmartMotion): identity matrix (no transform)
*/
#if (SM_BOARD_REV == SM_REVB_DB) /* when DB or EVB are used */
static int32_t icm_mounting_matrix[9] = { 0, -(1<<30), 0,
(1<<30), 0, 0,
0, 0, (1<<30) };
#else /* For SmartMotion */
static int32_t icm_mounting_matrix[9] = {(1<<30), 0, 0,
0, (1<<30), 0,
0, 0, (1<<30)};
#endif
bool custom_add_data; /* Custom data append flag (BLE transmission control) */
extern bool motion_raw_data_enabled; /* Flag requesting raw data read from external module */
extern char ble_tx_buffer[BLE_NUS_MAX_DATA_LEN]; /* BLE text transmit buffer */
extern which_cmd_t cmd_type_t; /* Current command source (BLE or UART) */
uint16_t ssp_data[6]={0,}; /* 6-axis data array for BLE (accel XYZ + gyro XYZ) */
extern bool info4; /* info4 mode flag (set by cmd_parse) */
volatile uint16_t info_imu[6]; /* Global array storing IMU data in info4 mode */
/* --------------------------------------------------------------------------------------
* static function declaration
* -------------------------------------------------------------------------------------- */
static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3]);
/* --------------------------------------------------------------------------------------
* Functions definition
* -------------------------------------------------------------------------------------- */
/*
* setup_imu_device()
* IMU device initialization and identification verification.
*
* Flow:
* 1) Initialize driver via inv_imu_init() (serial interface + callback registration)
* 2) Read WHOAMI register for device identification
* 3) Verify WHOAMI value matches ICM_WHOAMI (0x67)
*
* Returns: 0=success, negative=error
*/
int setup_imu_device(struct inv_imu_serif *icm_serif)
{
int rc = 0;
uint8_t who_am_i;
/* Initialize IMU driver — connect serial interface and register callback */
rc = inv_imu_init(&icm_driver, icm_serif, imu_callback);
if (rc != INV_ERROR_SUCCESS) {
DBG_PRINTF("!!! ERROR : Failed to initialize IMU!\r\n");
return rc;
}
/* Read WHOAMI register — verify device presence and communication */
rc = inv_imu_get_who_am_i(&icm_driver, &who_am_i);
if (rc != INV_ERROR_SUCCESS) {
DBG_PRINTF("!!! ERROR : Failed to read whoami!\r\n");
return rc;
}
/* Verify WHOAMI value — must be 0x67 for ICM42670P */
if (who_am_i != ICM_WHOAMI) {
DBG_PRINTF("!!! ERROR : Bad WHOAMI value! Read 0x%02x, expected 0x%02x\r\n", who_am_i, ICM_WHOAMI);
return INV_ERROR;
}
return rc;
}
/*
* configure_imu_device()
* Configures IMU sensor operating parameters.
*
* Settings:
* - FIFO: disabled (when USE_FIFO=0, direct register read mode)
* - Accel FSR: +/-4g (when USE_HIGH_RES_MODE=0)
* - Gyro FSR: +/-2000dps
* - ODR (output data rate):
* - Low-noise mode (USE_LOW_NOISE_MODE=1): 800Hz
* - Low-power mode (USE_LOW_NOISE_MODE=0): 100Hz
* - Gyro always operates in low-noise mode
* - Waits for gyro startup time when FIFO is not used
*
* Returns: 0=success, negative=error
*/
int configure_imu_device(void)
{
int rc = 0;
/* Disable FIFO — read data directly from registers */
if (!USE_FIFO)
rc |= inv_imu_configure_fifo(&icm_driver, INV_IMU_FIFO_DISABLED);
if (USE_HIGH_RES_MODE) {
/* High-resolution FIFO mode: 20-bit data, FSR locked to 16g/2000dps */
rc |= inv_imu_enable_high_resolution_fifo(&icm_driver);
} else {
/* Standard mode: accel +/-4g, gyro +/-2000dps FSR */
rc |= inv_imu_set_accel_fsr(&icm_driver, ACCEL_CONFIG0_FS_SEL_4g);
rc |= inv_imu_set_gyro_fsr(&icm_driver, GYRO_CONFIG0_FS_SEL_2000dps);
}
if (USE_LOW_NOISE_MODE) {
/* Low-noise mode: 800Hz ODR, enable accel low-noise mode */
rc |= inv_imu_set_accel_frequency(&icm_driver, ACCEL_CONFIG0_ODR_800_HZ);
rc |= inv_imu_set_gyro_frequency(&icm_driver, GYRO_CONFIG0_ODR_800_HZ);
rc |= inv_imu_enable_accel_low_noise_mode(&icm_driver);
} else {
/* Low-power mode: 100Hz ODR, enable accel low-power mode */
rc |= inv_imu_set_accel_frequency(&icm_driver, ACCEL_CONFIG0_ODR_100_HZ);
rc |= inv_imu_set_gyro_frequency(&icm_driver, GYRO_CONFIG0_ODR_100_HZ);
rc |= inv_imu_enable_accel_low_power_mode(&icm_driver);
}
/* Gyro always operates in low-noise mode regardless of setting */
rc |= inv_imu_enable_gyro_low_noise_mode(&icm_driver);
/* When FIFO is not used, wait for gyro startup time (delay until first valid data) */
if (!USE_FIFO)
inv_imu_sleep_us(GYR_STARTUP_TIME_US);
return rc;
}
/*
* get_imu_data()
* Reads sensor data from the IMU.
* Fetches data from FIFO or registers depending on USE_FIFO setting.
* Read data is processed via imu_callback().
*/
int get_imu_data(void)
{
#if USE_FIFO
return inv_imu_get_data_from_fifo(&icm_driver);
#else
return inv_imu_get_data_from_registers(&icm_driver);
#endif
}
#if SCALED_DATA_G_DPS
/*
* get_accel_and_gyr_fsr()
* Retrieves the currently configured FSR (Full Scale Range) for accel and gyro.
* Used for converting to scaled data (g, dps).
*/
static void get_accel_and_gyr_fsr(int16_t * accel_fsr_g, int16_t * gyro_fsr_dps)
{
ACCEL_CONFIG0_FS_SEL_t accel_fsr_bitfield;
GYRO_CONFIG0_FS_SEL_t gyro_fsr_bitfield;
inv_imu_get_accel_fsr(&icm_driver, &accel_fsr_bitfield);
switch(accel_fsr_bitfield) {
case ACCEL_CONFIG0_FS_SEL_2g: *accel_fsr_g = 2;
break;
case ACCEL_CONFIG0_FS_SEL_4g: *accel_fsr_g = 4;
break;
case ACCEL_CONFIG0_FS_SEL_8g: *accel_fsr_g = 8;
break;
case ACCEL_CONFIG0_FS_SEL_16g: *accel_fsr_g = 16;
break;
default: *accel_fsr_g = -1;
}
inv_imu_get_gyro_fsr(&icm_driver, &gyro_fsr_bitfield);
switch(gyro_fsr_bitfield) {
case GYRO_CONFIG0_FS_SEL_250dps: *gyro_fsr_dps = 250;
break;
case GYRO_CONFIG0_FS_SEL_500dps: *gyro_fsr_dps = 500;
break;
case GYRO_CONFIG0_FS_SEL_1000dps: *gyro_fsr_dps = 1000;
break;
case GYRO_CONFIG0_FS_SEL_2000dps: *gyro_fsr_dps = 2000;
break;
default: *gyro_fsr_dps = -1;
}
}
#endif
/*
* imu_callback()
* Callback invoked each time the IMU driver reads new sensor data.
*
* Flow:
* 1) Extract raw accel/gyro data from event
* - FIFO mode: handles timestamp rollover, supports high-res (20-bit)
* - Register mode: uses 16-bit data directly
* 2) Apply mounting matrix (board orientation correction)
* 3) Output data (branches by mode):
* - info4 mode: stores in info_imu[6] global array (polled externally)
* - UART mode: text output with "Tp" prefix for 6-axis data
* - BLE mode: binary packet with "rsp:" tag + simultaneous UART output
*/
void imu_callback(inv_imu_sensor_event_t *event)
{
int32_t accel[3], gyro[3];
#if SCALED_DATA_G_DPS
float accel_g[3];
float gyro_dps[3];
float temp_degc;
int16_t accel_fsr_g, gyro_fsr_dps;
#endif
#if USE_FIFO
static uint64_t last_fifo_timestamp = 0;
static uint32_t rollover_num = 0;
/* FIFO timestamp rollover handling (16-bit -> 64-bit extension) */
if (last_fifo_timestamp > event->timestamp_fsync)
rollover_num++;
last_fifo_timestamp = event->timestamp_fsync;
/* Convert timestamp to microseconds (apply Q24 resolution) */
timestamp = event->timestamp_fsync + rollover_num * UINT16_MAX;
timestamp *= inv_imu_get_fifo_timestamp_resolution_us_q24(&icm_driver);
timestamp /= (1UL << 24);
if (icm_driver.fifo_highres_enabled) {
/* High-res mode: left-shift 16-bit data by 4 + add lower 4 bits -> 20-bit */
accel[0] = (((int32_t)event->accel[0] << 4)) | event->accel_high_res[0];
accel[1] = (((int32_t)event->accel[1] << 4)) | event->accel_high_res[1];
accel[2] = (((int32_t)event->accel[2] << 4)) | event->accel_high_res[2];
gyro[0] = (((int32_t)event->gyro[0] << 4)) | event->gyro_high_res[0];
gyro[1] = (((int32_t)event->gyro[1] << 4)) | event->gyro_high_res[1];
gyro[2] = (((int32_t)event->gyro[2] << 4)) | event->gyro_high_res[2];
} else {
/* Standard resolution: use 16-bit data as-is */
accel[0] = event->accel[0];
accel[1] = event->accel[1];
accel[2] = event->accel[2];
gyro[0] = event->gyro[0];
gyro[1] = event->gyro[1];
gyro[2] = event->gyro[2];
}
#else
/* Direct register read mode: extract 16-bit raw data */
accel[0] = event->accel[0];
accel[1] = event->accel[1];
accel[2] = event->accel[2];
gyro[0] = event->gyro[0];
gyro[1] = event->gyro[1];
gyro[2] = event->gyro[2];
/* In register mode, force sensor mask so the output logic below works */
event->sensor_mask |= (1 << INV_SENSOR_TEMPERATURE);
event->sensor_mask |= (1 << INV_SENSOR_ACCEL);
event->sensor_mask |= (1 << INV_SENSOR_GYRO);
#endif
/* Apply mounting matrix — correct sensor physical orientation to software coordinates */
apply_mounting_matrix(icm_mounting_matrix, accel);
apply_mounting_matrix(icm_mounting_matrix, gyro);
#if SCALED_DATA_G_DPS
/*
* Convert raw data to physical units (g, dps)
* Formula: physical_value = raw_value * FSR / INT16_MAX
*/
get_accel_and_gyr_fsr(&accel_fsr_g, &gyro_fsr_dps);
accel_g[0] = (float)(accel[0] * accel_fsr_g) / INT16_MAX;
accel_g[1] = (float)(accel[1] * accel_fsr_g) / INT16_MAX;
accel_g[2] = (float)(accel[2] * accel_fsr_g) / INT16_MAX;
gyro_dps[0] = (float)(gyro[0] * gyro_fsr_dps) / INT16_MAX;
gyro_dps[1] = (float)(gyro[1] * gyro_fsr_dps) / INT16_MAX;
gyro_dps[2] = (float)(gyro[2] * gyro_fsr_dps) / INT16_MAX;
/* Temperature conversion: high-res/register mode uses /128, FIFO standard mode uses /2 */
if (USE_HIGH_RES_MODE || !USE_FIFO)
temp_degc = 25 + ((float)event->temperature / 128);
else
temp_degc = 25 + ((float)event->temperature / 2);
/*
* Output scaled data via UART
*/
if (event->sensor_mask & (1 << INV_SENSOR_ACCEL) && event->sensor_mask & (1 << INV_SENSOR_GYRO))
DBG_PRINTF("%u: %.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f\r\n",
(uint32_t)timestamp,
accel_g[0], accel_g[1], accel_g[2],
temp_degc,
gyro_dps[0], gyro_dps[1], gyro_dps[2]);
#else
/*
* Raw data output — branches by command source (info4/UART/BLE)
*/
if (event->sensor_mask & (1 << INV_SENSOR_ACCEL) && event->sensor_mask & (1 << INV_SENSOR_GYRO) || motion_raw_data_enabled)
{
motion_raw_data_enabled = false;
/* info4 mode: store data in global array info_imu[6], polled by external modules */
if (info4 == true)
{
info_imu[0] = (uint16_t)accel[0];
info_imu[1] = (uint16_t)accel[1];
info_imu[2] = (uint16_t)accel[2];
info_imu[3] = (uint16_t)gyro[0];
info_imu[4] = (uint16_t)gyro[1];
info_imu[5] = (uint16_t)gyro[2];
}
/* UART mode: output 6-axis data in text format with "Tp" prefix */
else if(cmd_type_t == CMD_UART) {
//DBG_PRINTF("Tp%d,%d,%d,%d,%d,%d\r\n\r\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
}
/*
* BLE mode: send 6-axis data as binary packet via BLE
* ssp_data[0..2] = accel XYZ, ssp_data[3..5] = gyro XYZ
* format_data() packs "rsp:" tag + 12-byte data
* dr_binary_tx_safe() sends 8 bytes via BLE
*/
else if(cmd_type_t == CMD_BLE) {
ssp_data[0] = (uint16_t)accel[0];
ssp_data[1] = (uint16_t)accel[1];
ssp_data[2] = (uint16_t)accel[2];
ssp_data[3] = (uint16_t)gyro[0];
ssp_data[4] = (uint16_t)gyro[1];
ssp_data[5] = (uint16_t)gyro[2];
format_data(imu_bin_buffer, "rsp:", ssp_data,12);
//DBG_PRINTF("Tp%d,%d,%d,%d,%d,%d\r\n\r\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
dr_binary_tx_safe(imu_bin_buffer,8);
if(custom_add_data==true) {
custom_add_data = false;
}
else {
//data_tx_handler(ble_tx_buffer);
}
}
}
#endif
}
/* --------------------------------------------------------------------------------------
* Static functions definition
* -------------------------------------------------------------------------------------- */
/*
* apply_mounting_matrix()
* Applies a Q30 fixed-point rotation matrix to a 3-axis vector.
*
* Calculation:
* result[i] = matrix[i*3+0]*raw[0] + matrix[i*3+1]*raw[1] + matrix[i*3+2]*raw[2]
* Right-shift result by 30 bits for Q30 -> integer conversion.
*
* Ensures a consistent coordinate system regardless of physical sensor orientation.
*/
static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3])
{
unsigned i;
int64_t data_q30[3];
for(i = 0; i < 3; i++) {
data_q30[i] = ((int64_t)matrix[3*i+0] * raw[0]);
data_q30[i] += ((int64_t)matrix[3*i+1] * raw[1]);
data_q30[i] += ((int64_t)matrix[3*i+2] * raw[2]);
}
/* Q30 -> integer conversion: right-shift by 30 bits */
raw[0] = (int32_t)(data_q30[0]>>30);
raw[1] = (int32_t)(data_q30[1]>>30);
raw[2] = (int32_t)(data_q30[2]>>30);
}
/*
* imu_read_direct()
* Reads IMU registers directly via I2C, bypassing the driver API.
* Reads data immediately without waiting for DRDY interrupt.
*
* Flow:
* 1) Check TWI initialization (first call only)
* 2) Gyro config: +/-2000dps, 100Hz ODR (GYRO_CONFIG0 = 0x09)
* 3) Accel config: +/-4g, 100Hz ODR (ACCEL_CONFIG0 = 0x29)
* 4) Power ON: accel+gyro low-noise mode (PWR_MGMT0 = 0x0F)
* 5) Wait 80ms (gyro startup: min 45ms + margin)
* 6) Read 12 consecutive bytes from ACCEL_DATA_X1 (0x0B) (accel 6 + gyro 6)
* 7) Big-endian -> int16_t conversion
* 8) Apply mounting matrix
* 9) Send via BLE with "rsp:" tag
* 10) Switch IMU to sleep mode (power saving)
*
* Returns: 0=success, -1=TX failure, -2=RX failure
*/
/* Raw I2C read from ICM42670P — bypasses driver API entirely */
#include "system_interface.h"
#include "nrfx_twi.h"
extern const nrfx_twi_t m_twi_icm42670;
#define IMU_I2C_ADDR 0x68
#define REG_ACCEL_X1 0x0B /* ACCEL_DATA_X1 — accel X-axis upper byte register */
/* --------------------------------------------------------------------------------------
* Direct IMU register read — raw I2C, no DRDY, sends rsp: via BLE
* Direct I2C register read (no interrupt, no IMU driver API)
* -------------------------------------------------------------------------------------- */
int imu_read_direct(void)
{
uint8_t raw[12]; /* accel 6 bytes + gyro 6 bytes */
int32_t accel[3], gyro[3];
uint8_t reg;
uint32_t ret;
static bool twi_ready = false;
/* TWI (I2C) init — performed only once (re-init ensures clean state) */
if (!twi_ready) {
inv_i2c_master_uninitialize();
inv_i2c_master_initialize();
twi_ready = true;
}
/* Gyro config: GYRO_CONFIG0(0x20) = 0x09 -> +/-2000dps FSR, 100Hz ODR */
{
uint8_t gyro_cfg[2] = { 0x20, 0x09 };
icm42670_twi_tx(IMU_I2C_ADDR, gyro_cfg, 2, false);
}
/* Accel config: ACCEL_CONFIG0(0x21) = 0x29 -> +/-4g FSR, 100Hz ODR */
{
uint8_t accel_cfg[2] = { 0x21, 0x29 };
icm42670_twi_tx(IMU_I2C_ADDR, accel_cfg, 2, false);
}
/* Power ON: PWR_MGMT0(0x1F) = 0x0F -> accel (low-noise) + gyro (low-noise) enabled */
{
uint8_t pwr_cmd[2] = { 0x1F, 0x0F }; /* reg=0x1F, val=0x0F */
icm42670_twi_tx(IMU_I2C_ADDR, pwr_cmd, 2, false);
//nrf_delay_ms(80); /* Gyro startup: min 45ms + safety margin */
dr_sd_delay_ms(80);
}
/* Read 12 consecutive bytes from ACCEL_DATA_X1 (0x0B~0x16) */
reg = REG_ACCEL_X1;
ret = icm42670_twi_tx(IMU_I2C_ADDR, &reg, 1, true); /* Send register address (no STOP) */
if (ret)
{
DBG_PRINTF("[IMU] tx FAIL %u\r\n", ret);
return -1;
}
ret = icm42670_twi_rx(IMU_I2C_ADDR, raw, 12); /* Receive 12 bytes of data */
if (ret)
{
DBG_PRINTF("[IMU] rx FAIL %u\r\n", ret);
return -2;
}
/*
* Convert big-endian register layout to int16_t
* raw[0..5] = accel X,Y,Z (2 bytes each, MSB first)
* raw[6..11] = gyro X,Y,Z (2 bytes each, MSB first)
*/
accel[0] = (int16_t)((raw[0] << 8) | raw[1]);
accel[1] = (int16_t)((raw[2] << 8) | raw[3]);
accel[2] = (int16_t)((raw[4] << 8) | raw[5]);
gyro[0] = (int16_t)((raw[6] << 8) | raw[7]);
gyro[1] = (int16_t)((raw[8] << 8) | raw[9]);
gyro[2] = (int16_t)((raw[10] << 8) | raw[11]);
/* Apply mounting matrix — board orientation correction */
apply_mounting_matrix(icm_mounting_matrix, accel);
apply_mounting_matrix(icm_mounting_matrix, gyro);
/* Pack data */
ssp_data[0] = (uint16_t)accel[0];
ssp_data[1] = (uint16_t)accel[1];
ssp_data[2] = (uint16_t)accel[2];
ssp_data[3] = (uint16_t)gyro[0];
ssp_data[4] = (uint16_t)gyro[1];
ssp_data[5] = (uint16_t)gyro[2];
if (info4 == true)
{
/* info4 mode: store in global array (sent as rbb: packet by mbb?) */
info_imu[0] = ssp_data[0];
info_imu[1] = ssp_data[1];
info_imu[2] = ssp_data[2];
info_imu[3] = ssp_data[3];
info_imu[4] = ssp_data[4];
info_imu[5] = ssp_data[5];
}
else
{
/* Normal mode: send immediately via BLE with "rsp:" tag */
format_data(imu_bin_buffer, "rsp:", ssp_data, 12);
dr_binary_tx_safe(imu_bin_buffer, 8);
}
/* IMU sleep mode: PWR_MGMT0 = 0x00 -> accel/gyro both OFF (power saving) */
{
uint8_t pwr_off[2] = { 0x1F, 0x00 }; /* reg=PWR_MGMT0, val=0x00 */
icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false);
}
return 0;
}
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.h
+101
View File
@@ -0,0 +1,101 @@
/*******************************************************************************
* @file app_raw.h
* @author CandyPops Co.
* @version V1.0.0
* @date 2022-09-05
* @brief
******************************************************************************/
/*******************************************************************************
* [Header overview] ICM42670P IMU driver application layer declarations
*
* Function prototypes and operating mode configuration macros for
* IMU sensor initialization, configuration, and data reading.
*
* Key configuration macros:
* SERIF_TYPE - Communication interface (UI_I2C)
* USE_LOW_NOISE_MODE - 1: low-noise (800Hz), 0: low-power (100Hz)
* USE_HIGH_RES_MODE - 1: 20-bit high-res, 0: 16-bit standard
* USE_FIFO - 1: use FIFO, 0: direct register read
******************************************************************************/
#ifndef _APP_RAW_H_
#define _APP_RAW_H_
#include "sdk_config.h"
#include <stdint.h>
#include "inv_imu_transport.h"
#include "inv_imu_defs.h"
#include "inv_imu_driver.h"
/*** Configuration macros ***/
/*
* MCU-IMU communication interface selection
* UI_I2C: use I2C communication (default)
*/
#define SERIF_TYPE UI_I2C
/*
* Power mode selection
* 1: Low-noise mode — 800Hz ODR, high precision, higher power consumption
* 0: Low-power mode — 100Hz ODR, lower power consumption
* Note: Low-noise mode cannot be used with ODR below 12.5Hz
*/
#define USE_LOW_NOISE_MODE 1
/*
* FIFO resolution mode selection
* 0: Low resolution — 16-bit data (default)
* 1: High resolution — 20-bit data (FSR locked to 16g/2000dps)
*/
#define USE_HIGH_RES_MODE 0
/*
* Data read method selection
* 0: Direct register read (currently in use)
* 1: Read from FIFO
*/
#define USE_FIFO 0
/**
* \brief Resets and initializes the IMU device. Includes WHOAMI verification.
* Must complete successfully before calling any other IMU access functions.
*
* \return 0=success, negative=error
*/
int setup_imu_device(struct inv_imu_serif *icm_serif);
/**
* \brief Configures the device for gyro and accel output.
* Applies FSR, ODR, power mode, and FIFO settings.
* \return 0=success, negative=error
*/
int configure_imu_device(void);
/**
* \brief Retrieves IMU data from FIFO or registers.
* Internally triggers imu_callback() for data processing.
* \return 0=success, negative=error
*/
int get_imu_data(void);
/**
* \brief Sensor data receive callback. Applies mounting matrix then
* outputs data according to info4/BLE/UART mode.
* \param[in] event Structure containing one sensor data packet
*/
void imu_callback(inv_imu_sensor_event_t *event);
/**
* \brief Direct I2C register read bypassing the driver API.
* Reads sensor data immediately without DRDY interrupt and sends via BLE.
* Switches IMU to sleep mode after reading to save power.
* \return 0=success, negative=error
*/
int imu_read_direct(void);
#endif /* !_APP_RAW_H_ */
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw_main.c
+292
View File
@@ -0,0 +1,292 @@
/*******************************************************************************
* @file app_raw_main.c
* @author CandyPops Co.
* @version V1.0.0
* @date 2022-09-05
* @brief
******************************************************************************/
/*******************************************************************************
* 2026.03.26 jhChun
* This file is currently not executed at runtime.
* Instead of interrupt-driven reads, imu_read_direct() in app_raw.c
* reads registers directly. May be cleaned up later as needed.
******************************************************************************/
/*******************************************************************************
* [Module overview] ICM42670P main initialization and polling loop
*
* Handles the full initialization sequence and main loop for the ICM42670P
* IMU sensor.
*
* Init flow (icm42670_init):
* 1) setup_mcu() - Configure I2C serial interface struct and TWI init
* 2) setup_imu_device() - IMU driver init + WHOAMI verification
* 3) configure_imu_device() - Sensor parameter config (FSR, ODR, power mode)
* 4) inv_gpio_sensor_irq_init() - INT1 (P1.13) GPIO interrupt setup
*
* Main loop (icm42670_main):
* - irq_from_device flag is set when INT1 interrupt fires
* - Main loop checks the flag and reads sensor data when set
* - Interrupt triggers on falling edge (HITOLO) with INT1 pin pulled up
*
* Helper functions:
* - inv_imu_sleep_us() - nrf_delay_us wrapper (used by IMU driver)
* - inv_imu_get_time_us() - Provides timestamp via RTC1 counter
******************************************************************************/
#include "sdk_config.h"
#include "app_raw.h"
#include "app_raw_main.h"
#include "RingBuffer.h"
#include "inv_imu_driver.h"
#include "system_interface.h"
/* std */
#include <stdio.h>
#include "nrf.h"
#include "app_error.h"
#include "boards.h"
#include "nrfx_gpiote.h"
#include "nrf_delay.h"
#include "app_util_platform.h"
#include "main.h" /* 2026-03-17: removed cmd_parse.h, using main.h */
#include "i2c_manager.h"
/* --------------------------------------------------------------------------------------
* Global variables
* -------------------------------------------------------------------------------------- */
/* --------------------------------------------------------------------------------------
* Static variables
* -------------------------------------------------------------------------------------- */
/*
* IMU interrupt flag
* Set to 1 on falling-edge interrupt of INT1 pin.
* Main loop checks this flag, reads data, and clears it to 0.
* volatile: modified by ISR, prevents compiler optimization.
*/
static volatile int irq_from_device;
/* --------------------------------------------------------------------------------------
* Forward declaration
* -------------------------------------------------------------------------------------- */
static int setup_mcu(struct inv_imu_serif *icm_serif);
/*!
* @brief Sensor general interrupt handler, calls specific handlers.
*
* This function is called when an external interrupt is triggered by the sensor,
* checks interrupt registers of InvenSense Sensor to determine the source and type of interrupt
* and calls the specific interrupt handler accordingly.
*
* @param[in] NULL
*
* @param[out] NULL
*
* @return NULL
*
*/
/*
* inv_gpio_sensor_interrupt_handler()
* INT1 pin interrupt handler (ISR).
* Called when the sensor has new data ready; sets a flag and returns immediately.
* Actual data processing is done in the main loop (icm42670_main).
*/
static void inv_gpio_sensor_interrupt_handler(nrfx_gpiote_pin_t pin, nrf_gpiote_polarity_t action)
{
irq_from_device = 1;
}
/*
* inv_gpio_sensor_irq_init()
* Initializes INT1 (P1.13) GPIO interrupt.
*
* Configuration:
* - Trigger: falling edge (HITOLO) — when the sensor pulls INT low
* - Pull-up: internal pull-up enabled
* - Handler: inv_gpio_sensor_interrupt_handler
* - Initializes GPIOTE module first if not already initialized
*/
void inv_gpio_sensor_irq_init(void)
{
ret_code_t err_code;
/* Initialize GPIOTE module (skip if already initialized) */
if (!nrfx_gpiote_is_init())
{
err_code = nrfx_gpiote_init();
APP_ERROR_CHECK(err_code);
}
/* Falling-edge interrupt: trigger on High->Low transition, internal pull-up */
nrfx_gpiote_in_config_t in_config = NRFX_GPIOTE_CONFIG_IN_SENSE_HITOLO(true);
in_config.pull = NRF_GPIO_PIN_PULLUP;
/* Register interrupt handler for INT1 pin */
err_code = nrfx_gpiote_in_init(ICM42670_INT1_PIN, &in_config, inv_gpio_sensor_interrupt_handler);
APP_ERROR_CHECK(err_code);
/* Enable interrupt event */
nrfx_gpiote_in_event_enable(ICM42670_INT1_PIN, true);
}
/*
* inv_gpio_sensor_irq_uninit()
* Disables and releases the INT1 GPIO interrupt.
* Called when deactivating the sensor or before re-initialization.
*/
void inv_gpio_sensor_irq_uninit(void)
{
/* Disable interrupt event */
nrfx_gpiote_in_event_disable(ICM42670_INT1_PIN);
/* Release INT1 pin interrupt configuration */
nrfx_gpiote_in_uninit(ICM42670_INT1_PIN);
/* Release GPIOTE module (only if initialized) */
if (nrfx_gpiote_is_init())
{
nrfx_gpiote_uninit();
}
}
/* --------------------------------------------------------------------------------------
* Main
* -------------------------------------------------------------------------------------- */
/*
* icm42670_init()
* Performs the full ICM42670P initialization sequence.
*
* Init order:
* 1) setup_mcu() - Configure I2C interface struct and TWI hardware init
* 2) setup_imu_device() - IMU driver init, WHOAMI (0x67) verification
* 3) configure_imu_device() - FSR, ODR, power mode configuration
* 4) inv_gpio_sensor_irq_init() - Enable INT1 interrupt (data-ready notification)
*
* Returns: 0=success, -1=initialization failure
*/
int icm42670_init(void)
{
int rc = 0;
struct inv_imu_serif icm_serif;
rc |= setup_mcu(&icm_serif);
rc |= setup_imu_device(&icm_serif);
rc |= configure_imu_device();
if(rc != 0){
printf("!!!error during initialization\r\n");
return -1;
}
/* Enable INT1 interrupt after successful init — ISR fires on data ready */
inv_gpio_sensor_irq_init();
return rc;
}
/*
* icm42670_main()
* ICM42670P main polling loop.
* Must be called periodically from the main application loop.
*
* Operation:
* 1) Check if I2C hardware is initialized (hw_i2c_init_once)
* 2) Check irq_from_device flag (set by ISR)
* 3) If flag is set, read sensor data (get_imu_data)
* 4) Clear flag after data read completes
*
* Note: Interrupt-based polling — ISR only sets the flag; actual I2C
* communication is done in the main context.
*/
void icm42670_main(void)
{
int rc = 0;
hw_i2c_init_once();
/* Check for interrupt and read data */
if (irq_from_device) {
rc = get_imu_data();
if(rc < 0) {
printf("error while getting data\r\n");
}
/* Clear flag — wait for next interrupt */
irq_from_device = 0;
}
}
/* --------------------------------------------------------------------------------------
* Functions definitions
* -------------------------------------------------------------------------------------- */
/*
* setup_mcu()
* Configures the MCU-side serial interface.
*
* Registers the following in the inv_imu_serif struct:
* - read_reg / write_reg : I2C read/write callbacks (implemented in system_interface.c)
* - max_read / max_write : Max transfer size (32KB)
* - serif_type : Communication type (UI_I2C)
*
* After configuration, calls inv_io_hal_init() to initialize the TWI hardware.
*/
static int setup_mcu(struct inv_imu_serif *icm_serif)
{
int rc = 0;
/* Configure serial interface struct for IMU driver */
icm_serif->context = 0; /* Context unused */
icm_serif->read_reg = inv_io_hal_read_reg; /* Register read callback */
icm_serif->write_reg = inv_io_hal_write_reg; /* Register write callback */
icm_serif->max_read = 1024*32; /* Max bytes per read */
icm_serif->max_write = 1024*32; /* Max bytes per write */
icm_serif->serif_type = SERIF_TYPE; /* UI_I2C (defined in app_raw.h) */
/* Initialize TWI hardware */
rc |= inv_io_hal_init(icm_serif);
return rc;
}
/* --------------------------------------------------------------------------------------
* Extern functions definition
* -------------------------------------------------------------------------------------- */
/*
* inv_imu_sleep_us()
* Microsecond sleep function used by the IMU driver.
* Wraps nrf_delay_us() to provide a platform-independent interface.
* Example: used for gyro startup delay (GYR_STARTUP_TIME_US).
*/
void inv_imu_sleep_us(uint32_t us)
{
nrf_delay_us(us);
}
/*
* inv_imu_get_time_us()
* Timestamp function used by the IMU driver.
* Returns the nRF52840 RTC1 counter value.
*
* Note: RTC1 runs at 32.768kHz, so the returned value is technically
* in RTC ticks (~30.5us/tick), not microseconds.
* Used for relative time comparisons within the driver.
*/
uint64_t inv_imu_get_time_us(void)
{
return NRF_RTC1->COUNTER;
}
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw_main.h
+32
View File
@@ -0,0 +1,32 @@
/*******************************************************************************
* @file app_raw_main.h
* @author CandyPops Co.
* @version V1.0.0
* @date 2022-09-05
* @brief
******************************************************************************/
/*******************************************************************************
* [Header overview] ICM42670P main initialization/polling loop declarations
*
* Declares the full initialization and main loop functions for the
* ICM42670P IMU sensor.
* - icm42670_init() : Full init (MCU config -> IMU init -> sensor config -> enable IRQ)
* - icm42670_main() : Main polling loop (check INT1 interrupt -> read data)
* - icm42670_uninit() : Release (prototype only, implementation elsewhere)
******************************************************************************/
#ifndef _APP_RAW_MAIN_H_
#define _APP_RAW_MAIN_H_
#include "sdk_config.h"
/* ICM42670P full init — MCU I2C config -> IMU driver init -> sensor config -> enable IRQ */
int icm42670_init(void);
/* ICM42670P main polling loop — check INT1 interrupt flag, then read sensor data */
void icm42670_main(void);
/* ICM42670P release (prototype declaration) */
int icm42670_uninit(void);
#endif /* !_APP_RAW_MAIN_H_ */
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/system_interface.c
+334
View File
@@ -0,0 +1,334 @@
/*******************************************************************************
* @file system_interface.c
* @author CandyPops Co.
* @version V1.0.0
* @date 2022-09-05
* @brief
******************************************************************************/
/*******************************************************************************
* [Module overview] ICM42670P IMU sensor I2C communication interface
*
* Low-level interface module for communicating with the ICM42670P IMU sensor
* via the nRF52840 TWI (I2C) hardware.
*
* - I2C slave address: 0x68 (ICM42670P default)
* - I2C pin config: SCL=P1.14, SDA=P1.15 (defined in system_interface.h)
* - TWI instance: NRFX_TWI_INSTANCE(0)
* - Bus speed: 100kHz (NRF_TWI_FREQ_100K)
*
* Main function flow:
* inv_io_hal_init() -> Initialize I2C or SPI (only I2C implemented)
* inv_io_hal_read_reg() -> Register read (TX address -> RX data)
* inv_io_hal_write_reg() -> Register write (TX address+data at once)
*
* Error handling: All I2C read/write operations retry once on failure
*
* Note: SPI4 code path exists but is not implemented;
* only I2C (UI_I2C) is used in production.
******************************************************************************/
/* board driver */
#include <stdio.h>
#include <string.h>
#include <stdarg.h>
#include <stdbool.h>
#include "nrf.h"
#include "app_error.h"
#include "boards.h"
#include "nrfx_gpiote.h"
#include "nrfx_twi.h"
#include "system_interface.h"
#include "nrf_delay.h"
/* ICM42670P I2C slave address and max serial write byte count */
#define ICM_I2C_ADDR 0x68
#define INV_MAX_SERIAL_WRITE 16
/* TWI (I2C) instance — uses ICM42670_I2C_INSTANCE(0) from system_interface.h */
const nrfx_twi_t m_twi_icm42670 = NRFX_TWI_INSTANCE(ICM42670_I2C_INSTANCE);
/*
* inv_i2c_master_uninitialize()
* Disables the I2C bus and releases the TWI instance.
* Called before entering sleep mode or before re-initialization.
*/
void inv_i2c_master_uninitialize(void){
nrfx_twi_disable(&m_twi_icm42670);
nrfx_twi_uninit(&m_twi_icm42670);
}
/*
* inv_i2c_master_initialize()
* Initializes and enables the nRF52840 TWI hardware.
* - SCL: P1.14, SDA: P1.15
* - Speed: 100kHz
* - Interrupt priority: highest (APP_IRQ_PRIORITY_HIGH)
* - No event handler (blocking mode)
*/
void inv_i2c_master_initialize(void){
ret_code_t err_code;
const nrfx_twi_config_t twi_icm42670_config = {
.scl = ICM42670_I2C_SCL_PIN,
.sda = ICM42670_I2C_SDA_PIN,
.frequency = NRF_TWI_FREQ_100K,
.interrupt_priority = APP_IRQ_PRIORITY_HIGH,
};
/* Initialize TWI driver (event handler=NULL -> blocking mode) */
err_code = nrfx_twi_init(&m_twi_icm42670, &twi_icm42670_config, NULL, NULL);
APP_ERROR_CHECK(err_code);
/* Enable TWI hardware — tx/rx available after this */
nrfx_twi_enable(&m_twi_icm42670);
}
/*
* icm42670_twi_tx()
* I2C transmit wrapper. Calls nrfx_twi_tx to send data.
* If no_stop=true, STOP condition is omitted (used for Repeated START).
*/
uint32_t icm42670_twi_tx( uint8_t device_id,
uint8_t const * p_data,
uint8_t length,
bool no_stop)
{
ret_code_t ret;
ret = nrfx_twi_tx(&m_twi_icm42670, device_id, p_data, length, no_stop);
return ret;
}
/*
* icm42670_twi_rx()
* I2C receive wrapper. Calls nrfx_twi_rx to receive data.
*/
uint32_t icm42670_twi_rx( uint8_t device_id,
uint8_t * p_data,
uint8_t length)
{
ret_code_t ret;
ret = nrfx_twi_rx(&m_twi_icm42670, device_id, p_data, length);
return ret;
}
/*
* inv_i2c_master_read_register()
* Reads data from a specific ICM42670P register.
*
* Sequence:
* 1) TX: Send 1-byte register address (no_stop=true -> prepare Repeated START)
* 2) RX: Receive data of specified length
*
* Error handling: Retries once on TX or RX failure.
*/
static unsigned long inv_i2c_master_read_register(unsigned char Address, unsigned char RegisterAddr, unsigned short RegisterLen, unsigned char *RegisterValue){
//ret_code_t ret;
uint32_t ret;
uint8_t addr8 = (uint8_t)RegisterAddr;
/* Step 1: Send register address to read (no STOP -> uses Repeated START) */
ret = icm42670_twi_tx(Address, &addr8, 1, true);
if(ret != NRF_SUCCESS) {
/* Retry once on failure */
ret = icm42670_twi_tx(Address, &addr8, 1, true);
if(ret != NRF_SUCCESS) {
printf("ERR! i2c read-1\r\n");
}
}
/* Step 2: Receive data from the register */
ret = icm42670_twi_rx(Address, RegisterValue, RegisterLen);
if(ret != NRF_SUCCESS) {
/* Retry once on failure */
ret = icm42670_twi_rx(Address, RegisterValue, RegisterLen);
if(ret != NRF_SUCCESS) {
printf("ERR! i2c read-2\r\n");
}
}
return ret;
}
/*
* inv_i2c_master_write_register()
* Writes data to a specific ICM42670P register.
*
* Sequence:
* 1) Place register address in buffer[0], data in buffer[1..N]
* 2) TX: Send address+data at once (no_stop=false -> includes STOP condition)
*
* Error handling: Retries once on failure.
*/
static unsigned long inv_i2c_master_write_register(unsigned char Address, unsigned char RegisterAddr, unsigned short RegisterLen, const unsigned char *RegisterValue){
uint32_t ret;
uint8_t buffer[1 + INV_MAX_SERIAL_WRITE]; /* register address (1) + data (max 16 bytes) */
/* Buffer layout: [register address][data bytes] */
buffer[0] = (uint8_t)RegisterAddr;
memcpy(buffer+1, RegisterValue, RegisterLen);
/* Send address+data at once */
ret = icm42670_twi_tx(Address, buffer, RegisterLen+1, false);
if(ret != NRF_SUCCESS) {
/* Retry once on failure */
ret = icm42670_twi_tx(Address, buffer, RegisterLen+1, false);
if(ret != NRF_SUCCESS) {
printf("ERR! i2c write\r\n");
}
}
return ret;
}
/*
* inv_io_hal_init()
* Initializes the serial interface (I2C or SPI) used by the IMU driver.
* Branches based on serif->serif_type; only I2C is currently implemented.
* Returns: 0=success, -1=unsupported interface type
*/
int inv_io_hal_init(struct inv_imu_serif *serif)
{
switch (serif->serif_type) {
case UI_SPI4:
{
/* SPI4 init — not implemented (only I2C is used) */
break;
}
case UI_I2C:
inv_i2c_master_initialize();
break;
default:
return -1;
}
return 0;
}
/*
* inv_io_hal_read_reg()
* IMU driver callback: reads data from the specified register.
* Performs I2C or SPI read depending on the serial type.
*/
int inv_io_hal_read_reg(struct inv_imu_serif *serif, uint8_t reg, uint8_t * rbuffer, uint32_t rlen)
{
switch (serif->serif_type) {
case UI_SPI4:
return 0;
case UI_I2C:
return inv_i2c_master_read_register(ICM_I2C_ADDR, reg, rlen, rbuffer);
default:
return -1;
}
}
/*
* inv_io_hal_write_reg()
* IMU driver callback: writes data to the specified register.
* Performs I2C or SPI write depending on the serial type.
*/
int inv_io_hal_write_reg(struct inv_imu_serif *serif, uint8_t reg, const uint8_t * wbuffer, uint32_t wlen)
{
switch (serif->serif_type) {
case UI_SPI4:
return 0;
case UI_I2C:
return inv_i2c_master_write_register(ICM_I2C_ADDR, reg, wlen, wbuffer);
default:
return -1;
}
}
/*
* cat_read()
* Generic I2C read function (debug/legacy).
* Reads 8 bytes, returns the first byte, and prints the data to console.
* Note: Not used in production; kept for debug purposes.
*/
uint8_t cat_read(uint8_t device_id, uint8_t address, uint8_t *data)
{
uint8_t read_data = 0;
char adata[8];
ret_code_t err_code;
//address = 1|(address<<1);
address = (address & 0xFF);
/* Send register address (no STOP, prepare Repeated START) */
err_code = nrfx_twi_tx(&m_twi_icm42670, device_id, &address, 1, true);
if (err_code != NRF_SUCCESS) {
// Handle error
// return;
}
/* Receive 8 bytes of data */
err_code = nrfx_twi_rx(&m_twi_icm42670, device_id, data, 8);
if (err_code != NRF_SUCCESS) {
// Handle error
return 0;
}
read_data = data[0];
memcpy(adata,data,8);
printf("Data %s . \r\n", adata);
return read_data;
}
/*
* cat_write()
* Generic I2C write function (debug/legacy).
* Sends 1 byte address + 1 byte data.
* Note: Copies 6 bytes into buffer, but only transmits 2 bytes.
*/
void cat_write(uint8_t device_id, uint8_t address, uint8_t *data){
uint8_t buffer[7]={0x00,0x00,0x00,0x00,0x00,0x00,0x00};
address = (address & 0xFF);
buffer[0] = (address);
//buffer[1] =(data & 0xFF);
memcpy(buffer+1,data,6);
ret_code_t err_code;
//err_code = nrf_drv_twi_tx(&m_twi_ir, device_id, 0x00, 1, false);
/* Address (1 byte) + data (1 byte) = 2 bytes transmitted */
err_code = nrfx_twi_tx(&m_twi_icm42670, device_id, buffer, 2, false);
// err_code = nrf_drv_twi_tx(&m_twi_ir, device_id, buffer, 2, false);
// nrfx_twi_rx(&m_twi_icm42670, device_id, p_data, length);
// nrfx_twi_tx(&m_twi_icm42670, device_id, p_data, length, no_stop);
printf("Data %x %x %x %x. \r\n", buffer[0], buffer[1], buffer[2], buffer[3]);
//err_code = nrf_drv_twi_tx(&m_twi_ir, device_id, buffer, 6, false);
if (err_code != NRF_SUCCESS) {
printf("TWI Error.");
}
}
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/system_interface.h
+73
View File
@@ -0,0 +1,73 @@
/*******************************************************************************
* @file system_interface.h
* @author CandyPops Co.
* @version V1.0.0
* @date 2022-09-05
* @brief
******************************************************************************/
/*******************************************************************************
* [Header overview] ICM42670P I2C communication interface declarations
*
* Pin definitions and function prototypes for communicating with the
* ICM42670P IMU sensor via nRF52840 TWI hardware.
*
* Pin assignment:
* - I2C SCL : P1.14
* - I2C SDA : P1.15
* - INT1 : P1.13 (data-ready interrupt)
* - INT2 : P0.26 (auxiliary interrupt, currently unused)
*
* TWI instance: 0
******************************************************************************/
#ifndef _SYSTEM_INTERFACE_H_
#define _SYSTEM_INTERFACE_H_
#include "inv_imu_transport.h"
#include <stdbool.h>
/* TODO: Move that somewhere else */
#ifndef TO_MASK
#define TO_MASK(a) (1U << (unsigned)(a))
#endif
#define ICM42670_I2C_INSTANCE 0 /**< I2C (TWI) instance index */
#define ICM42670_I2C_SDA_PIN NRF_GPIO_PIN_MAP(1,15) /**< SDA pin: P1.15 */
#define ICM42670_I2C_SCL_PIN NRF_GPIO_PIN_MAP(1,14) /**< SCL pin: P1.14 */
#define ICM42670_INT1_PIN NRF_GPIO_PIN_MAP(1,13) /**< INT1 pin: P1.13 (data-ready interrupt) */
#define ICM42670_INT2_PIN NRF_GPIO_PIN_MAP(0,26) /**< INT2 pin: P0.26 (auxiliary, currently unused) */
/* I2C transmit wrapper — if no_stop=true, STOP condition is omitted for Repeated START */
uint32_t icm42670_twi_tx( uint8_t device_id,
uint8_t const * p_data,
uint8_t length,
bool no_stop);
/* I2C receive wrapper */
uint32_t icm42670_twi_rx( uint8_t device_id,
uint8_t * p_data,
uint8_t length);
/* Generic I2C read (debug/legacy) — reads 8 bytes and returns the first byte */
uint8_t cat_read (uint8_t device_id, uint8_t address, uint8_t *data);
/* Generic I2C write (debug/legacy) — sends address+data 2 bytes */
void cat_write (uint8_t device_id, uint8_t address, uint8_t *data);
/* Release I2C hardware (called before sleep or re-initialization) */
void inv_i2c_master_uninitialize(void);
/* Initialize I2C hardware (100kHz, blocking mode) */
void inv_i2c_master_initialize(void);
/* Initialize serial interface for IMU driver (I2C/SPI branch) */
int inv_io_hal_init(struct inv_imu_serif *serif);
/* IMU driver callback: register read */
int inv_io_hal_read_reg(struct inv_imu_serif *serif, uint8_t reg, uint8_t * rbuffer, uint32_t rlen);
/* IMU driver callback: register write */
int inv_io_hal_write_reg(struct inv_imu_serif *serif, uint8_t reg, const uint8_t * wbuffer, uint32_t wlen);
#endif /* !_SYSTEM_INTERFACE_H_ */
project/ble_peripheral/ble_app_bladder_patch/measurement/piezo/dr_piezo.c
+1535
View File
File diff suppressed because it is too large Load Diff
project/ble_peripheral/ble_app_bladder_patch/measurement/piezo/dr_piezo.h
+125
View File
@@ -0,0 +1,125 @@
/*==============================================================================
* dr_piezo.h - Piezo Transducer Driver (2 MHz Signal Generator)
*
* Hardware: nRF52840 + MD1822K6-G MOSFET Driver + TC7920K6-G MOSFET
* Output: +/-20V at 2 MHz, 3..7 cycles burst
*
* Timing Sequence:
* 1. PE = HIGH (enable)
* 2. P_OUT/N_OUT = 2 MHz pulses (3..7 cycles)
* 3. DMP = HIGH (dump residual energy)
* 4. DMP = LOW
* 5. PE = LOW (disable)
*
* Pin assignment:
* Power: DR_PIEZO_PWR_EN (P1.9) — DC/DC +/-20V enable
* TX: PE (P0.25), DMP (P1.0), P_OUT (P1.7), N_OUT (P1.6)
* MUX: EN_MUXA (P0.21), EN_MUXB (P0.23), SEL0 (P1.10), SEL1 (P0.28)
*
* MUX channel mapping (8ch):
* CH0=A0(1,0,0,0) CH1=A2(1,0,1,0) CH2=A1(1,0,0,1) CH3=A3(1,0,1,1)
* CH4=B0(0,1,1,1) CH5=B1(0,1,0,1) CH6=B2(0,1,1,0) CH7=B3(0,1,0,0)
*
* Two burst modes:
* 1) HW burst (dr_piezo_burst): Timer2 + PPI + GPIOTE, CPU-independent
* 2) SW burst (dr_piezo_burst_sw_XXmhz): CPU NOP-based precise timing
* Per-frequency functions: 1.7 / 1.8 / 1.9 / 2.0 / 2.1 / 2.2 MHz
*============================================================================*/
#ifndef DR_PIEZO_H
#define DR_PIEZO_H
#include <stdint.h>
#include <stdbool.h>
#include "nrf_gpio.h"
/*==============================================================================
* Power control pin (+/-20V DC/DC converter)
*============================================================================*/
#define DR_PIEZO_PWR_EN NRF_GPIO_PIN_MAP(1, 9)
/*==============================================================================
* TX signal pins (MOSFET driver)
* PE: Pulse Enable — activates the entire TX sequence
* DMP: Dump — discharges residual piezo energy after burst
* P_OUT: Positive output — drives piezo positive terminal
* N_OUT: Negative output — drives piezo negative terminal (inverted P_OUT)
*============================================================================*/
#define DR_PIEZO_PIN_PE NRF_GPIO_PIN_MAP(0, 25) /**< Pulse Enable */
#define DR_PIEZO_PIN_DMP NRF_GPIO_PIN_MAP(1, 0) /**< Dump control */
#define DR_PIEZO_PIN_P_OUT NRF_GPIO_PIN_MAP(1, 7) /**< Positive output */
#define DR_PIEZO_PIN_N_OUT NRF_GPIO_PIN_MAP(1, 6) /**< Negative output */
/*==============================================================================
* MUX control pins (echo signal path selection, 8 channels)
* MUXA handles CH0..CH3, MUXB handles CH4..CH7.
* Only one MUX is enabled at a time.
*============================================================================*/
#define DR_PIEZO_EN_MUXA NRF_GPIO_PIN_MAP(0, 21) /**< MUXA Enable */
#define DR_PIEZO_EN_MUXB NRF_GPIO_PIN_MAP(0, 23) /**< MUXB Enable */
#define DR_PIEZO_MUX_SEL0 NRF_GPIO_PIN_MAP(1, 10) /**< MUX Select 0 */
#define DR_PIEZO_MUX_SEL1 NRF_GPIO_PIN_MAP(0, 28) /**< MUX Select 1 */
/*==============================================================================
* Configuration
*============================================================================*/
#define DR_PIEZO_FREQ_HZ 2100000 /**< Target frequency (set PIEZO_FREQ_MHZ in .c) */
#define DR_PIEZO_DEFAULT_CYCLES 5 /**< Default burst cycles */
#define DR_PIEZO_MIN_CYCLES 3
#define DR_PIEZO_MAX_CYCLES 7
#define DR_PIEZO_MUX_SETTLING_US 1300 /**< MUX settling delay (us) */
/*==============================================================================
* Power control
*============================================================================*/
void dr_piezo_power_on(void);
void dr_piezo_power_off(void);
/** @return true if power is ON */
bool dr_piezo_is_power_on(void);
/*==============================================================================
* TX driver
*============================================================================*/
void dr_piezo_init(void);
void dr_piezo_uninit(void);
void dr_piezo_burst(uint8_t cycles);
void dr_piezo_pulse(void);
void dr_piezo_enable(void);
void dr_piezo_disable(void);
bool dr_piezo_is_busy(void);
void dr_piezo_set_frequency(uint32_t freq_hz);
void dr_piezo_test_pins(void);
void dr_piezo_mux_init(void);
/**
* @brief Select piezo channel (0..7) via 8ch MUX
* @note MUX settling time: ~1.3 ms delay after switching
*/
void dr_piezo_select_channel(uint8_t channel);
/*==============================================================================
* System functions (power + TX combined)
*============================================================================*/
void dr_piezo_system_init(void);
void dr_piezo_system_uninit(void);
void dr_piezo_transmit(uint8_t cycles);
/*==============================================================================
* Software burst — CPU NOP-based precise timing, no Timer/PPI
*
* Interrupts are disabled during burst for timing accuracy.
* Per-frequency functions (NOP count varies):
*============================================================================*/
void dr_piezo_burst_sw(uint8_t cycles); /**< 2.1 MHz (default) */
void dr_piezo_burst_sw_18mhz(uint8_t cycles); /**< 1.8 MHz */
void dr_piezo_burst_sw_20mhz(uint8_t cycles); /**< 2.0 MHz */
void dr_piezo_burst_sw_22mhz(uint8_t cycles); /**< 2.2 MHz */
void dr_piezo_burst_sw_17mhz(uint8_t cycles); /**< 1.7 MHz */
void dr_piezo_burst_sw_19mhz(uint8_t cycles); /**< 1.9 MHz */
#endif /* DR_PIEZO_H */
project/ble_peripheral/ble_app_bladder_patch/measurement/temperature/tmp235_q1.c
+168
View File
@@ -0,0 +1,168 @@
/*==============================================================================
* tmp235_q1.c - TMP235-Q1 analogue temperature sensor driver
*
* Reads the TMP235-Q1 analogue output via SAADC AIN3 and converts to deg C.
*
* Temperature conversion (piecewise linear, per datasheet):
* Vout <= 1500 mV (0..100 C) : Ta = (Vout - 500) / 10.0
* Vout <= 1750 mV (100..125 C): Ta = (Vout - 1500) / 10.1 + 100
* Vout <= 2000 mV (125..150 C): Ta = (Vout - 1752.5) / 10.6 + 125
* Vout > 2000 mV : out of sensor range
*============================================================================*/
#include "sdk_common.h"
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include "nrf.h"
#include "boards.h"
#include "app_error.h"
#include "nrf_drv_saadc.h"
#include "ble_nus.h"
#include "tmp235_q1.h"
#include "main.h"
#include "main_timer.h"
#include "battery_saadc.h"
#include "debug_print.h"
/* SAADC internal reference (mV) */
#define TMP235_REF_VOLTAGE_IN_MILLIVOLTS 600.0f
/* 1/3 prescaling compensation (x6) */
#define TMP235_PRE_SCALING_COMPENSATION 6.0f
/* 12-bit ADC full scale */
#define TMP235_ADC_RES_12BITS 4096.0f
/* Convert raw ADC value to TMP235 output voltage (mV) */
#define TMP235_VOUT_IN_MILLI_VOLTS(ADC_VALUE)\
((((ADC_VALUE) * TMP235_REF_VOLTAGE_IN_MILLIVOLTS) / TMP235_ADC_RES_12BITS) * TMP235_PRE_SCALING_COMPENSATION)
static nrf_saadc_value_t adc_buf;
extern char ble_tx_buffer[BLE_NUS_MAX_DATA_LEN];
extern uint8_t ble_bin_buffer[BLE_NUS_MAX_DATA_LEN];
extern which_cmd_t cmd_type_t;
extern bool info4;
extern bool go_temp;
/* info4 mode: cached temperature (deg C x 100, integer) */
volatile uint16_t info_temp;
extern bool motion_raw_data_enabled;
/* SAADC completion flag — used by all_sensors() to wait */
volatile bool tmp235_saadc_done = false;
/*==============================================================================
* tmp235_voltage_handler - SAADC conversion complete callback
*
* ADC value -> Vout (mV) -> temperature (deg C), then:
* - info4 mode: store to info_temp (C x 100 integer)
* - Normal mode: send rso: response over BLE or UART
*============================================================================*/
void tmp235_voltage_handler(nrf_drv_saadc_evt_t const * p_event)
{
float led_temp;
float led_temp_16;
if (p_event->type == NRF_DRV_SAADC_EVT_DONE)
{
nrf_saadc_value_t adc_result;
float tmp235_voltage_in_milli_volts = 0;
adc_result = p_event->data.done.p_buffer[0];
/* Release SAADC — shared with battery / pressure ADC */
nrf_drv_saadc_channel_uninit(0);
nrf_drv_saadc_uninit();
/* ADC -> TMP235 output voltage (mV) */
tmp235_voltage_in_milli_volts = TMP235_VOUT_IN_MILLI_VOLTS(adc_result);
/* Vout -> temperature (piecewise linear per datasheet) */
if(tmp235_voltage_in_milli_volts <= 1500)
{
/* 0..100 C: slope 10.0 mV/C, offset 500 mV */
led_temp = (tmp235_voltage_in_milli_volts - 500.0f) / 10.0f + 0.0f;
}
else if(tmp235_voltage_in_milli_volts <= 1750)
{
/* 100..125 C: slope 10.1 mV/C */
led_temp = (tmp235_voltage_in_milli_volts - 1500.0f) / 10.1f + 100.0f;
}
else if(tmp235_voltage_in_milli_volts <= 2000)
{
/* 125..150 C: slope 10.6 mV/C */
led_temp = (tmp235_voltage_in_milli_volts - 1752.5f) / 10.6f + 125.0f;
}
else
{
/* Out of sensor range (>150 C) */
DBG_PRINTF("ERR!!! Temperature is over 150c\r\n");
}
/* --- Safety check mode: pass temperature to battery module for judgment --- */
if (safety_check_mode == true)
{
safety_check_mode = false;
safety_check_complete(led_temp);
}
/* --- info4 mode: store value for mbb? bulk response --- */
else if (info4 == true)
{
info_temp = (uint16_t)(led_temp * 100);
}
else if (cmd_type_t == CMD_UART)
{
DBG_PRINTF("To%.2f\r\n\r\n", led_temp);
}
else if (cmd_type_t == CMD_BLE)
{
led_temp_16 = led_temp * 100;
single_format_data(ble_bin_buffer, "rso:", (uint16_t)led_temp_16);
dr_binary_tx_safe(ble_bin_buffer, 3);
}
tmp235_saadc_done = true;
}
}
/*==============================================================================
* tmp235_init - Initialise SAADC for TMP235 and start measurement
*
* AIN3, single-ended, 12-bit, 4x oversampling, burst enabled.
* Triggers sampling immediately; result arrives via tmp235_voltage_handler.
*============================================================================*/
void tmp235_init(void)
{
nrf_drv_saadc_config_t saadc_config = NRF_DRV_SAADC_DEFAULT_CONFIG;
saadc_config.resolution = NRF_SAADC_RESOLUTION_12BIT;
saadc_config.oversample = NRF_SAADC_OVERSAMPLE_4X;
ret_code_t err_code = nrf_drv_saadc_init(&saadc_config, tmp235_voltage_handler);
APP_ERROR_CHECK(err_code);
nrf_saadc_channel_config_t config = NRF_DRV_SAADC_DEFAULT_CHANNEL_CONFIG_SE(NRF_SAADC_INPUT_AIN3);
config.burst = NRF_SAADC_BURST_ENABLED;
err_code = nrf_drv_saadc_channel_init(0, &config);
APP_ERROR_CHECK(err_code);
err_code = nrf_drv_saadc_buffer_convert(&adc_buf, 1);
APP_ERROR_CHECK(err_code);
err_code = nrf_drv_saadc_sample();
APP_ERROR_CHECK(err_code);
}
/*==============================================================================
* tmp235_voltage_level_meas - External entry point for one-shot reading
*
* Calls tmp235_init() which both initialises and triggers sampling.
*============================================================================*/
void tmp235_voltage_level_meas(void)
{
tmp235_init();
}
project/ble_peripheral/ble_app_bladder_patch/measurement/temperature/tmp235_q1.h
+22
View File
@@ -0,0 +1,22 @@
/*==============================================================================
* tmp235_q1.h - TMP235-Q1 analogue temperature sensor driver interface
*
* Reads the TMP235-Q1 analogue voltage output via SAADC (AIN3) and converts
* it to temperature (deg C).
*
* Conversion: Ta(C) = (Vout_mV - 500) / 10.0 (valid 0..100 C)
*
* API:
* tmp235_init() : initialise SAADC + start measurement (internal)
* tmp235_voltage_level_meas() : one-shot temperature reading (external wrapper)
*============================================================================*/
#ifndef _TMP235_Q1_H_
#define _TMP235_Q1_H_
/* Initialise SAADC for TMP235 and start measurement (AIN3). */
void tmp235_init(void);
/* External entry point for a single temperature reading. */
void tmp235_voltage_level_meas(void);
#endif /* !_TMP235_Q1_H_ */