Exportable version of WizziLab's modem driver.
include/kal_math.h
- Committer:
- Jeej
- Date:
- 2021-01-27
- Revision:
- 56:67e3d9608403
- Parent:
- 31:517fc900afba
File content as of revision 56:67e3d9608403:
/// @copyright /// ========================================================================={{{ /// Copyright (c) 2013-2014 WizziLab / /// All rights reserved / /// / /// IMPORTANT: This Software may not be modified, copied or distributed unless / /// embedded on a WizziLab product. Other than for the foregoing purpose, this / /// Software and/or its documentation may not be used, reproduced, copied, / /// prepared derivative works of, modified, performed, distributed, displayed / /// or sold for any purpose. For the sole purpose of embedding this Software / /// on a WizziLab product, copy, modification and distribution of this / /// Software is granted provided that the following conditions are respected: / /// / /// * Redistributions of source code must retain the above copyright notice, / /// this list of conditions and the following disclaimer / /// / /// * Redistributions in binary form must reproduce the above copyright / /// notice, this list of conditions and the following disclaimer in the / /// documentation and/or other materials provided with the distribution. / /// / /// * The name of WizziLab can not be used to endorse or promote products / /// derived from this software without specific prior written permission. / /// / /// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS / /// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED / /// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR / /// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR / /// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, / /// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, / /// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, / /// OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY / /// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING / /// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS / /// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. / /// WIZZILAB HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, / /// ENHANCEMENTS OR MODIFICATIONS. / /// / /// Should you have any questions regarding your right to use this Software, / /// contact WizziLab at www.wizzilab.com. / /// / /// =========================================================================}}} /// @endcopyright #ifndef __KAL_MATH_H__ #define __KAL_MATH_H__ #include "hal_types.h" // ============================================================================= // Compare operators // ============================================================================= #define KAL_MAX(a,b) (((a) > (b)) ? (a) : (b)) #define KAL_MIN(a,b) (((a) < (b)) ? (a) : (b)) //#define KAL_ABS(a) (((a) > 0) ? (a) : (-(a))) #define KAL_ABS16_FAST(a) ((a) ^ ((a) >> 15)) #define KAL_ABS32_FAST(a) ((a) ^ ((a) >> 31)) #define KAL_SAT_RANGE(a,min,max) (((a) < (min)) ? (min) : ((a) > (max)) ? (max) : (a)) #define KAL_SAT_S16(a) (((a) < MIN_S16) ? MIN_S16 : ((a) > MAX_S16) ? MAX_S16 : (s16)(a)) #define KAL_SAT_U16(a) (((a) > MAX_U16) ? MAX_U16 : (u16)(a)) #define KAL_SAT_S8(a) (((a) < MIN_S8) ? MIN_S8 : ((a) > MAX_S8) ? MAX_S8 : (s8)(a)) #define KAL_SAT_U8(a) (((a) > MAX_U8) ? MAX_U8 : (u8)(a)) #define KAL_COMP16_FAST(a,b) (((b) - (a)) >> 15) // ============================================================================= // Bit manipulation operators // ============================================================================= #define KAL_BIT_CLR(d, j) (d)[(j)/8] &= (~(1 << ((j) & 7))) #define KAL_BIT_SET(d, j) (d)[(j)/8] |= (((1) << ((j) & 7))) #define KAL_BIT_MOV(s, i, d, j) (d)[(j)/8] |= ((((s)[(i)/8]>>((i)&7))&1)<<((j)&7)) #define KAL_BIT_IS_ONE(s, i) ((s)[(i)/8] & (1 << ((i) & 7))) #define KAL_BIT_GET(s, i) ((s)[(i)/8] >> ((i) & 7) & 1) #define KAL_2BIT_SH(i) (2 * ((i) & 3)) #define KAL_2BIT_GET(d, i) (((d)[(i)/4] >> KAL_2BIT_SH(i)) & 3) #define KAL_2BIT_CLR(d, i) ((d)[(i)/4] &=~ (3 << KAL_2BIT_SH(i))) #define KAL_2BIT_SET(d, i, c) ((d)[(i)/4] |= ((c) << KAL_2BIT_SH(i))) #define KAL_2BIT_MOV(s, i, d, j) KAL_2BIT_SET(d, j, KAL_2BIT_GET(s, i)) // ============================================================================= // Simple arithmetic operators // ============================================================================= #define KAL_MUL(x,y) ((x)*(y)) #define KAL_MLF(x,y,frac) (((x)*(y)) >> (frac)) #define KAL_SHIFT_LEFT_SIGNED(a,s) (((s) > 0) ? ((a) << (s)) : ((a) >> (-(s)))) #define KAL_SQR(x) KAL_MUL(x,x) #define KAL_DIV_INT(n,d) (((n) + ((d)/2))/(d)) #define KAL_DIV_CEILING(n,d) (((n) + (d) - 1)/(d)) #define KAL_DIV_FLOOR(n,d) (((n) )/(d)) // ============================================================================= // Complex operators // ============================================================================= #define KAL_COMPLEX_ADD(a,b,out) { (out).I = (a).I + (b).I; (out).Q = (a).Q + (b).Q; } #define KAL_COMPLEX_SUB(a,b,out) { (out).I = (a).I - (b).I; (out).Q = (a).Q - (b).Q; } #define KAL_COMPLEX_MUL(a,b,out) { (out).I = KAL_MUL((a).I,(b).I) - KAL_MUL((a).Q,(b).Q); \ (out).Q = KAL_MUL((a).I,(b).Q) + KAL_MUL((a).Q,(b).I); } #define KAL_COMPLEX_MLF(a,b,out,frac) { (out).I = KAL_MLF((a).I,(b).I,(frac)) - KAL_MLF((a).Q,(b).Q,(frac)); \ (out).Q = KAL_MLF((a).I,(b).Q,(frac)) + KAL_MLF((a).Q,(b).I,(frac)); } #define KAL_COMPLEX_NORM(a) (KAL_MUL((a).I,(a).I) + KAL_MUL((a).Q,(a).Q)) #define KAL_COMPLEX_MOD(a) (kal_sqrt32((u32)KAL_COMPLEX_NORM(a))) #define KAL_COMPLEX_DIST(a,b,res) { \ complex_t diff; \ KAL_COMPLEX_SUB((a),(b), diff); \ (res) = KAL_COMPLEX_MOD(diff); \ } //====================================================================== // Circular d-dimensional buffer structure //====================================================================== typedef struct { // Number of dimensions of the buffer u8 dim; // Buffer length (number of vectors) u8 len; // Current vector position in the buffer u8 curr; // Pointer to the start of the buffer s16 buf[1]; } kal_circ_buf_t; // ======================================================================= // kal_abs_s32 // ----------------------------------------------------------------------- /// @brief Absolute function /// @param x s32 input /// @retval u32 output // ======================================================================= INLINE _public u32 kal_abs_s32(s32 x) { return (u32)((x > 0)? x : -x); } INLINE _public u16 kal_abs_s16(s16 x) { return (u16)((x > 0)? x : -x); } INLINE _public u8 kal_abs_s8(s8 x) { return (u8)((x > 0)? x : -x); } // ======================================================================= // kal_sqrt // ----------------------------------------------------------------------- /// @brief Fixed point square root /// @param x u16 input fixed point value /// @param format u8 fixed point format (shift) /// @retval u16 square root of the input in the given format // ======================================================================= _public u16 kal_sqrt(u16 x, u8 format); // ======================================================================= // kal_sqrt32 // ----------------------------------------------------------------------- /// @brief Integer square root /// @param x u32 input fixed point value /// @retval u16 square root of the input // ======================================================================= _public u16 kal_sqrt32(u32 x); // ======================================================================= // kal_div // ----------------------------------------------------------------------- /// @brief Fixed point division of two s16 values /// @param nom s16 Numerator /// @param denom s16 Denominator /// @param format u8 fixed point format (shift) /// @retval s16 division result in the given format // ======================================================================= _public s16 kal_div(s16 num, s16 denom, u8 format); // ======================================================================= // kal_div_u32 // ----------------------------------------------------------------------- /// @brief Fixed point division of two u32 values /// @param n u32 Numerator /// @param d u32 Denominator /// @param format u8 fixed point format (shift) /// @retval u32 division result in the given format // ======================================================================= _public u32 kal_div_u32(u32 n, u32 d, u8 format); // ======================================================================= // kal_log2 // ----------------------------------------------------------------------- /// @brief Fast log2 computation /// @param in u32 operand /// @retval u8 log2 of the operand // ======================================================================= _public u8 kal_log2(u32 in); // ======================================================================= // kal_complex_arg // ----------------------------------------------------------------------- /// @brief Compute the argument of a complex number /// @param in u32 operand /// @retval u16 angle in degrees [0, 360[ // ======================================================================= _public u16 kal_complex_arg(complex_t in); // ======================================================================= // kal_sin // ----------------------------------------------------------------------- /// @brief sin(a) in Q15 /// @param a u16 angle in degrees [0, 360[ /// @retval s16 sin(a) in Q15 // ======================================================================= _public u16 kal_sin(u16 a); // ======================================================================= // kal_cos // ----------------------------------------------------------------------- /// @brief cos(a) in Q15 /// @param a u16 angle in degrees [0, 360[ /// @retval s16 cos(a) in Q15 // ======================================================================= #define kal_cos(a) kal_sin(((450-(a)) % 360)) // ======================================================================= // kal_add // ----------------------------------------------------------------------- /// @brief Add B to A and store in A /// @param a s16* pointer to the d-dimensional vector A /// @param b s16* pointer to the d-dimensional vector B /// @param d u8 number of dimensions of the buffer /// @retval void // ======================================================================= _public void kal_add(s16* a, s16* b, u8 d); // ======================================================================= // kal_sub // ----------------------------------------------------------------------- /// @brief Subtract B from A and store in A /// @param a s16* pointer to the d-dimensional vector A /// @param b s16* pointer to the d-dimensional vector B /// @param d u8 number of dimensions of the buffer /// @retval void // ======================================================================= _public void kal_sub(s16* a, s16* b, u8 d); // ======================================================================= // kal_mean // ----------------------------------------------------------------------- /// @brief Get mean of data in a d-dimensional buffer /// @param in s16* input circular buffer /// @param out s16* pointer to the d-dimensional result /// @param len u8 length of the buffer /// @param d u8 number of dimensions of the buffer /// @retval void // ======================================================================= _public void kal_mean(s16* in, s16* out, u8 len, u8 d); // ======================================================================= // kal_var // ----------------------------------------------------------------------- /// @brief Get the combined variance of data in a d-dimensional buffer /// @param in s16* input circular buffer /// @param mean s16* pointer to the d-dimensional mean /// @param len u8 length of the buffer /// @param d u8 number of dimensions of the buffer /// @retval u32 variance // ======================================================================= _public u32 kal_var(s16* in, s16* mean, u8 len, u8 d); // ======================================================================= // kal_dev // ----------------------------------------------------------------------- /// @brief Get the deviation per axis in a d-dimensional buffer /// @param buf s16* input circular buffer /// @param inout s16* [in] pointer to the d-dimensional mean /// [out] pointer to the d-dimensional dev /// @param len u8 length of the buffer /// @param d u8 number of dimensions of the buffer /// @retval void // ======================================================================= _public void kal_dev(s16* buf, s16* inout, u8 len, u8 d); // ======================================================================= // kal_norm // ----------------------------------------------------------------------- /// @brief Get norm of an d-dimensional vector /// @param in s16* d-dimensional vector /// @param d u8 number of dimensions of the vector /// @retval u32 norm of the vector // ======================================================================= _public u32 kal_norm(s16* in, u8 d); // ======================================================================= // kal_dist // ----------------------------------------------------------------------- /// @brief Get distance between two d-dimensional vectors /// @param a s16* first d-dimensional vector /// @param b s16* second d-dimensional vector /// @param d u8 number of dimensions of the vector /// @retval u32 distance (norm of the difference) // ======================================================================= _public u32 kal_dist(s16* a, s16* b, u8 d); //====================================================================== // kal_circ_buf_alloc //---------------------------------------------------------------------- /// @brief Allocate and init circular buffer /// @param len u8 Number of d-dimensional vectors in the buffer /// @param dim u8 Number of dimensions of the buffer /// @return kal_circ_buf_t* Pointer to the circular buffer structure //====================================================================== _public kal_circ_buf_t* kal_circ_buf_alloc(u8 len, u8 dim); //====================================================================== // kal_circ_buf_free //---------------------------------------------------------------------- /// @brief Free circular buffer /// @param circ kal_circ_buf_t* Pointer to the circular buffer structure /// @return void //====================================================================== _public void kal_circ_buf_free(kal_circ_buf_t* circ); //====================================================================== // kal_circ_buf_init //---------------------------------------------------------------------- /// @brief Init the buffer with the same element /// @param v s16* d-dimensional vector to write /// @param circ kal_circ_buf_t* Pointer to the circular buffer structure /// @return void //====================================================================== _public void kal_circ_buf_init(s16* v, kal_circ_buf_t* circ); //====================================================================== // kal_circ_buf_add //---------------------------------------------------------------------- /// @brief Add new element of type s16 in buffer, erase oldest element /// @param v s16* d-dimensional vector to write /// @param circ kal_circ_buf_t* Pointer to the circular buffer structure /// @return void //====================================================================== _public void kal_circ_buf_add(s16* v, kal_circ_buf_t* circ); //====================================================================== // kal_circ_buf_stats //---------------------------------------------------------------------- /// @brief Mean and variance of the circular buffer /// @param v s16* d-dimensional mean vector (output) /// @param circ kal_circ_buf_t* Pointer to the circular buffer structure /// @retval u32 variance //====================================================================== _public u32 kal_circ_buf_stats(s16* mean, kal_circ_buf_t* circ); //====================================================================== // kal_lp_filter //---------------------------------------------------------------------- /// @brief Low Pass Filter with forget factor /// @param lp s16* Pointer to the LP sample /// @param s s16* Pointer to the new sample /// @param dim u8 Number of dimensions of the sample /// @param ff u8 Forget factor /// @return void //====================================================================== _public void kal_lp_filter(s16* lpf, s16* s, u8 dim, u8 ff); //====================================================================== // kal_xor //---------------------------------------------------------------------- /// @brief xor two vectors /// @param buf u8* inout stream /// @param iv u8* in stream to XOR /// @retval void //====================================================================== _public void kal_xor(u8* buf, u8* iv, u16 len); //====================================================================== // kal_sort //---------------------------------------------------------------------- /// @brief Sort u16 buffer from smaller to bigger error /// @param inout u16* inout buffer /// @param ref u16 comparison reference /// @param len u8 buffer length /// @return void //====================================================================== _public void kal_sort(u16* inout, u16 ref, u8 len); #endif // __KAL_MATH_H__