initial commit
This commit is contained in:
jhChun
@@ -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));
|
||||
}
|
||||
}
|
||||
@@ -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_ */
|
||||
|
||||
/** @} */
|
||||
@@ -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";
|
||||
}
|
||||
}
|
||||
@@ -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_ */
|
||||
|
||||
/** @} */
|
||||
@@ -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]);
|
||||
}
|
||||
|
||||
|
||||
@@ -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_ */
|
||||
|
||||
/** @} */
|
||||
|
||||
@@ -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
|
||||
|
||||
/** \} */
|
||||
Reference in New Issue
Block a user