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

Initial commit: MT firmware project

Browse Source 
- BLE peripheral applications
- dr_piezo and bladder_patch projects

Co-Authored-By: Claude Opus 4.5 <noreply@anthropic.com>
This commit is contained in:
Charles Kwon
2026-01-25 17:26:39 +09:00
commit 72f5eb3cd9
2559 changed files with 1594625 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/EmbUtils/DataConverter.c Normal file
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));
}
}

87
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/EmbUtils/DataConverter.h Normal file
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_ */
/** @} */

47
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/EmbUtils/ErrorHelper.c Normal file
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";
}
}

51
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/EmbUtils/ErrorHelper.h Normal file
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_ */
/** @} */

82
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/EmbUtils/InvBasicMath.c Normal file
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]);
}

94
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/EmbUtils/InvBasicMath.h Normal file
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_ */
/** @} */

389
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/EmbUtils/RingBuffer.h Normal file
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
/** \} */

47
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/InvBool.h Normal file
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_ */
/** @} */

64
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/InvError.h Normal file
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_ */
/** @} */

39
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/InvExport.h Normal file
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_ */

370
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/imu/inv_imu_apex.c Normal file
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;
}

185
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/imu/inv_imu_apex.h Normal file
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_ */
/** @} */

1011
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/imu/inv_imu_defs.h Normal file
View File

File diff suppressed because it is too large Load Diff

1619
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/imu/inv_imu_driver.c Normal file
View File

File diff suppressed because it is too large Load Diff

520
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/imu/inv_imu_driver.h Normal file
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_ */
/** @} */

64
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/imu/inv_imu_extfunc.h Normal file
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_ */
/** @} */

3391
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/imu/inv_imu_regmap.h Normal file
View File

File diff suppressed because it is too large Load Diff

179
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/imu/inv_imu_selftest.c Normal file
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;
}

101
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/imu/inv_imu_selftest.h Normal file
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_ */
/** @} */

257
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/imu/inv_imu_transport.c Normal file
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;
}

122
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/imu/inv_imu_transport.h Normal file
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_ */
/** @} */

37
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/imu/inv_imu_version.h Normal file
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_ */

153
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/lib_agm/invn/common/invn_types.h Normal file
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_

156
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/lib_agm/invn_algo_agm.h Normal file
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

BIN
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/lib_agm/libInvnAlgoAGM.a Normal file
View File

Binary file not shown.

3012
project/ble_peripheral/ble_app_bladder_patch/icm42670p/Invn/lib_agm/test_vector/out_invn_agm_1.h Normal file
View File

File diff suppressed because it is too large Load Diff