WizziLab
/
modem_ref
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
kal_math.h
00001 /// @copyright 00002 /// ========================================================================={{{ 00003 /// Copyright (c) 2013-2014 WizziLab / 00004 /// All rights reserved / 00005 /// / 00006 /// IMPORTANT: This Software may not be modified, copied or distributed unless / 00007 /// embedded on a WizziLab product. Other than for the foregoing purpose, this / 00008 /// Software and/or its documentation may not be used, reproduced, copied, / 00009 /// prepared derivative works of, modified, performed, distributed, displayed / 00010 /// or sold for any purpose. For the sole purpose of embedding this Software / 00011 /// on a WizziLab product, copy, modification and distribution of this / 00012 /// Software is granted provided that the following conditions are respected: / 00013 /// / 00014 /// * Redistributions of source code must retain the above copyright notice, / 00015 /// this list of conditions and the following disclaimer / 00016 /// / 00017 /// * Redistributions in binary form must reproduce the above copyright / 00018 /// notice, this list of conditions and the following disclaimer in the / 00019 /// documentation and/or other materials provided with the distribution. / 00020 /// / 00021 /// * The name of WizziLab can not be used to endorse or promote products / 00022 /// derived from this software without specific prior written permission. / 00023 /// / 00024 /// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS / 00025 /// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED / 00026 /// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR / 00027 /// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR / 00028 /// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, / 00029 /// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, / 00030 /// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, / 00031 /// OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY / 00032 /// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING / 00033 /// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS / 00034 /// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. / 00035 /// WIZZILAB HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, / 00036 /// ENHANCEMENTS OR MODIFICATIONS. / 00037 /// / 00038 /// Should you have any questions regarding your right to use this Software, / 00039 /// contact WizziLab at www.wizzilab.com. / 00040 /// / 00041 /// =========================================================================}}} 00042 /// @endcopyright 00043 00044 #ifndef __KAL_MATH_H__ 00045 #define __KAL_MATH_H__ 00046 00047 #include "hal_types.h" 00048 00049 // ============================================================================= 00050 // Compare operators 00051 // ============================================================================= 00052 00053 #define KAL_MAX(a,b) (((a) > (b)) ? (a) : (b)) 00054 #define KAL_MIN(a,b) (((a) < (b)) ? (a) : (b)) 00055 00056 //#define KAL_ABS(a) (((a) > 0) ? (a) : (-(a))) 00057 #define KAL_ABS16_FAST(a) ((a) ^ ((a) >> 15)) 00058 #define KAL_ABS32_FAST(a) ((a) ^ ((a) >> 31)) 00059 00060 #define KAL_SAT_RANGE(a,min,max) (((a) < (min)) ? (min) : ((a) > (max)) ? (max) : (a)) 00061 #define KAL_SAT_S16(a) (((a) < MIN_S16) ? MIN_S16 : ((a) > MAX_S16) ? MAX_S16 : (s16)(a)) 00062 #define KAL_SAT_U16(a) (((a) > MAX_U16) ? MAX_U16 : (u16)(a)) 00063 #define KAL_SAT_S8(a) (((a) < MIN_S8) ? MIN_S8 : ((a) > MAX_S8) ? MAX_S8 : (s8)(a)) 00064 #define KAL_SAT_U8(a) (((a) > MAX_U8) ? MAX_U8 : (u8)(a)) 00065 00066 #define KAL_COMP16_FAST(a,b) (((b) - (a)) >> 15) 00067 00068 // ============================================================================= 00069 // Bit manipulation operators 00070 // ============================================================================= 00071 00072 #define KAL_BIT_CLR(d, j) (d)[(j)/8] &= (~(1 << ((j) & 7))) 00073 #define KAL_BIT_SET(d, j) (d)[(j)/8] |= (((1) << ((j) & 7))) 00074 #define KAL_BIT_MOV(s, i, d, j) (d)[(j)/8] |= ((((s)[(i)/8]>>((i)&7))&1)<<((j)&7)) 00075 #define KAL_BIT_IS_ONE(s, i) ((s)[(i)/8] & (1 << ((i) & 7))) 00076 #define KAL_BIT_GET(s, i) ((s)[(i)/8] >> ((i) & 7) & 1) 00077 00078 #define KAL_2BIT_SH(i) (2 * ((i) & 3)) 00079 #define KAL_2BIT_GET(d, i) (((d)[(i)/4] >> KAL_2BIT_SH(i)) & 3) 00080 #define KAL_2BIT_CLR(d, i) ((d)[(i)/4] &=~ (3 << KAL_2BIT_SH(i))) 00081 #define KAL_2BIT_SET(d, i, c) ((d)[(i)/4] |= ((c) << KAL_2BIT_SH(i))) 00082 #define KAL_2BIT_MOV(s, i, d, j) KAL_2BIT_SET(d, j, KAL_2BIT_GET(s, i)) 00083 00084 // ============================================================================= 00085 // Simple arithmetic operators 00086 // ============================================================================= 00087 00088 #define KAL_MUL(x,y) ((x)*(y)) 00089 #define KAL_MLF(x,y,frac) (((x)*(y)) >> (frac)) 00090 #define KAL_SHIFT_LEFT_SIGNED(a,s) (((s) > 0) ? ((a) << (s)) : ((a) >> (-(s)))) 00091 #define KAL_SQR(x) KAL_MUL(x,x) 00092 #define KAL_DIV_INT(n,d) (((n) + ((d)/2))/(d)) 00093 #define KAL_DIV_CEILING(n,d) (((n) + (d) - 1)/(d)) 00094 #define KAL_DIV_FLOOR(n,d) (((n) )/(d)) 00095 00096 // ============================================================================= 00097 // Complex operators 00098 // ============================================================================= 00099 00100 #define KAL_COMPLEX_ADD(a,b,out) { (out).I = (a).I + (b).I; (out).Q = (a).Q + (b).Q; } 00101 #define KAL_COMPLEX_SUB(a,b,out) { (out).I = (a).I - (b).I; (out).Q = (a).Q - (b).Q; } 00102 00103 #define KAL_COMPLEX_MUL(a,b,out) { (out).I = KAL_MUL((a).I,(b).I) - KAL_MUL((a).Q,(b).Q); \ 00104 (out).Q = KAL_MUL((a).I,(b).Q) + KAL_MUL((a).Q,(b).I); } 00105 00106 #define KAL_COMPLEX_MLF(a,b,out,frac) { (out).I = KAL_MLF((a).I,(b).I,(frac)) - KAL_MLF((a).Q,(b).Q,(frac)); \ 00107 (out).Q = KAL_MLF((a).I,(b).Q,(frac)) + KAL_MLF((a).Q,(b).I,(frac)); } 00108 00109 #define KAL_COMPLEX_NORM(a) (KAL_MUL((a).I,(a).I) + KAL_MUL((a).Q,(a).Q)) 00110 #define KAL_COMPLEX_MOD(a) (kal_sqrt32((u32)KAL_COMPLEX_NORM(a))) 00111 00112 #define KAL_COMPLEX_DIST(a,b,res) { \ 00113 complex_t diff; \ 00114 KAL_COMPLEX_SUB((a),(b), diff); \ 00115 (res) = KAL_COMPLEX_MOD(diff); \ 00116 } 00117 00118 //====================================================================== 00119 // Circular d-dimensional buffer structure 00120 //====================================================================== 00121 typedef struct 00122 { 00123 // Number of dimensions of the buffer 00124 u8 dim; 00125 // Buffer length (number of vectors) 00126 u8 len; 00127 // Current vector position in the buffer 00128 u8 curr; 00129 // Pointer to the start of the buffer 00130 s16 buf[1]; 00131 00132 } kal_circ_buf_t; 00133 00134 // ======================================================================= 00135 // kal_abs_s32 00136 // ----------------------------------------------------------------------- 00137 /// @brief Absolute function 00138 /// @param x s32 input 00139 /// @retval u32 output 00140 // ======================================================================= 00141 INLINE _public u32 kal_abs_s32(s32 x) 00142 { 00143 return (u32)((x > 0)? x : -x); 00144 } 00145 00146 INLINE _public u16 kal_abs_s16(s16 x) 00147 { 00148 return (u16)((x > 0)? x : -x); 00149 } 00150 00151 INLINE _public u8 kal_abs_s8(s8 x) 00152 { 00153 return (u8)((x > 0)? x : -x); 00154 } 00155 00156 // ======================================================================= 00157 // kal_sqrt 00158 // ----------------------------------------------------------------------- 00159 /// @brief Fixed point square root 00160 /// @param x u16 input fixed point value 00161 /// @param format u8 fixed point format (shift) 00162 /// @retval u16 square root of the input in the given format 00163 // ======================================================================= 00164 _public u16 kal_sqrt(u16 x, u8 format); 00165 00166 // ======================================================================= 00167 // kal_sqrt32 00168 // ----------------------------------------------------------------------- 00169 /// @brief Integer square root 00170 /// @param x u32 input fixed point value 00171 /// @retval u16 square root of the input 00172 // ======================================================================= 00173 _public u16 kal_sqrt32(u32 x); 00174 00175 // ======================================================================= 00176 // kal_div 00177 // ----------------------------------------------------------------------- 00178 /// @brief Fixed point division of two s16 values 00179 /// @param nom s16 Numerator 00180 /// @param denom s16 Denominator 00181 /// @param format u8 fixed point format (shift) 00182 /// @retval s16 division result in the given format 00183 // ======================================================================= 00184 _public s16 kal_div(s16 num, s16 denom, u8 format); 00185 00186 // ======================================================================= 00187 // kal_div_u32 00188 // ----------------------------------------------------------------------- 00189 /// @brief Fixed point division of two u32 values 00190 /// @param n u32 Numerator 00191 /// @param d u32 Denominator 00192 /// @param format u8 fixed point format (shift) 00193 /// @retval u32 division result in the given format 00194 // ======================================================================= 00195 _public u32 kal_div_u32(u32 n, u32 d, u8 format); 00196 00197 // ======================================================================= 00198 // kal_log2 00199 // ----------------------------------------------------------------------- 00200 /// @brief Fast log2 computation 00201 /// @param in u32 operand 00202 /// @retval u8 log2 of the operand 00203 // ======================================================================= 00204 _public u8 kal_log2(u32 in); 00205 00206 // ======================================================================= 00207 // kal_complex_arg 00208 // ----------------------------------------------------------------------- 00209 /// @brief Compute the argument of a complex number 00210 /// @param in u32 operand 00211 /// @retval u16 angle in degrees [0, 360[ 00212 // ======================================================================= 00213 _public u16 kal_complex_arg(complex_t in); 00214 00215 // ======================================================================= 00216 // kal_sin 00217 // ----------------------------------------------------------------------- 00218 /// @brief sin(a) in Q15 00219 /// @param a u16 angle in degrees [0, 360[ 00220 /// @retval s16 sin(a) in Q15 00221 // ======================================================================= 00222 _public u16 kal_sin(u16 a); 00223 00224 // ======================================================================= 00225 // kal_cos 00226 // ----------------------------------------------------------------------- 00227 /// @brief cos(a) in Q15 00228 /// @param a u16 angle in degrees [0, 360[ 00229 /// @retval s16 cos(a) in Q15 00230 // ======================================================================= 00231 #define kal_cos(a) kal_sin(((450-(a)) % 360)) 00232 00233 // ======================================================================= 00234 // kal_add 00235 // ----------------------------------------------------------------------- 00236 /// @brief Add B to A and store in A 00237 /// @param a s16* pointer to the d-dimensional vector A 00238 /// @param b s16* pointer to the d-dimensional vector B 00239 /// @param d u8 number of dimensions of the buffer 00240 /// @retval void 00241 // ======================================================================= 00242 _public void kal_add(s16* a, s16* b, u8 d); 00243 00244 // ======================================================================= 00245 // kal_sub 00246 // ----------------------------------------------------------------------- 00247 /// @brief Subtract B from A and store in A 00248 /// @param a s16* pointer to the d-dimensional vector A 00249 /// @param b s16* pointer to the d-dimensional vector B 00250 /// @param d u8 number of dimensions of the buffer 00251 /// @retval void 00252 // ======================================================================= 00253 _public void kal_sub(s16* a, s16* b, u8 d); 00254 00255 // ======================================================================= 00256 // kal_mean 00257 // ----------------------------------------------------------------------- 00258 /// @brief Get mean of data in a d-dimensional buffer 00259 /// @param in s16* input circular buffer 00260 /// @param out s16* pointer to the d-dimensional result 00261 /// @param len u8 length of the buffer 00262 /// @param d u8 number of dimensions of the buffer 00263 /// @retval void 00264 // ======================================================================= 00265 _public void kal_mean(s16* in, s16* out, u8 len, u8 d); 00266 00267 // ======================================================================= 00268 // kal_var 00269 // ----------------------------------------------------------------------- 00270 /// @brief Get the combined variance of data in a d-dimensional buffer 00271 /// @param in s16* input circular buffer 00272 /// @param mean s16* pointer to the d-dimensional mean 00273 /// @param len u8 length of the buffer 00274 /// @param d u8 number of dimensions of the buffer 00275 /// @retval u32 variance 00276 // ======================================================================= 00277 _public u32 kal_var(s16* in, s16* mean, u8 len, u8 d); 00278 00279 // ======================================================================= 00280 // kal_dev 00281 // ----------------------------------------------------------------------- 00282 /// @brief Get the deviation per axis in a d-dimensional buffer 00283 /// @param buf s16* input circular buffer 00284 /// @param inout s16* [in] pointer to the d-dimensional mean 00285 /// [out] pointer to the d-dimensional dev 00286 /// @param len u8 length of the buffer 00287 /// @param d u8 number of dimensions of the buffer 00288 /// @retval void 00289 // ======================================================================= 00290 _public void kal_dev(s16* buf, s16* inout, u8 len, u8 d); 00291 00292 // ======================================================================= 00293 // kal_norm 00294 // ----------------------------------------------------------------------- 00295 /// @brief Get norm of an d-dimensional vector 00296 /// @param in s16* d-dimensional vector 00297 /// @param d u8 number of dimensions of the vector 00298 /// @retval u32 norm of the vector 00299 // ======================================================================= 00300 _public u32 kal_norm(s16* in, u8 d); 00301 00302 // ======================================================================= 00303 // kal_dist 00304 // ----------------------------------------------------------------------- 00305 /// @brief Get distance between two d-dimensional vectors 00306 /// @param a s16* first d-dimensional vector 00307 /// @param b s16* second d-dimensional vector 00308 /// @param d u8 number of dimensions of the vector 00309 /// @retval u32 distance (norm of the difference) 00310 // ======================================================================= 00311 _public u32 kal_dist(s16* a, s16* b, u8 d); 00312 00313 //====================================================================== 00314 // kal_circ_buf_alloc 00315 //---------------------------------------------------------------------- 00316 /// @brief Allocate and init circular buffer 00317 /// @param len u8 Number of d-dimensional vectors in the buffer 00318 /// @param dim u8 Number of dimensions of the buffer 00319 /// @return kal_circ_buf_t* Pointer to the circular buffer structure 00320 //====================================================================== 00321 _public kal_circ_buf_t* kal_circ_buf_alloc(u8 len, u8 dim); 00322 00323 //====================================================================== 00324 // kal_circ_buf_free 00325 //---------------------------------------------------------------------- 00326 /// @brief Free circular buffer 00327 /// @param circ kal_circ_buf_t* Pointer to the circular buffer structure 00328 /// @return void 00329 //====================================================================== 00330 _public void kal_circ_buf_free(kal_circ_buf_t* circ); 00331 00332 //====================================================================== 00333 // kal_circ_buf_init 00334 //---------------------------------------------------------------------- 00335 /// @brief Init the buffer with the same element 00336 /// @param v s16* d-dimensional vector to write 00337 /// @param circ kal_circ_buf_t* Pointer to the circular buffer structure 00338 /// @return void 00339 //====================================================================== 00340 _public void kal_circ_buf_init(s16* v, kal_circ_buf_t* circ); 00341 00342 //====================================================================== 00343 // kal_circ_buf_add 00344 //---------------------------------------------------------------------- 00345 /// @brief Add new element of type s16 in buffer, erase oldest element 00346 /// @param v s16* d-dimensional vector to write 00347 /// @param circ kal_circ_buf_t* Pointer to the circular buffer structure 00348 /// @return void 00349 //====================================================================== 00350 _public void kal_circ_buf_add(s16* v, kal_circ_buf_t* circ); 00351 00352 //====================================================================== 00353 // kal_circ_buf_stats 00354 //---------------------------------------------------------------------- 00355 /// @brief Mean and variance of the circular buffer 00356 /// @param v s16* d-dimensional mean vector (output) 00357 /// @param circ kal_circ_buf_t* Pointer to the circular buffer structure 00358 /// @retval u32 variance 00359 //====================================================================== 00360 _public u32 kal_circ_buf_stats(s16* mean, kal_circ_buf_t* circ); 00361 00362 //====================================================================== 00363 // kal_lp_filter 00364 //---------------------------------------------------------------------- 00365 /// @brief Low Pass Filter with forget factor 00366 /// @param lp s16* Pointer to the LP sample 00367 /// @param s s16* Pointer to the new sample 00368 /// @param dim u8 Number of dimensions of the sample 00369 /// @param ff u8 Forget factor 00370 /// @return void 00371 //====================================================================== 00372 _public void kal_lp_filter(s16* lpf, s16* s, u8 dim, u8 ff); 00373 00374 //====================================================================== 00375 // kal_xor 00376 //---------------------------------------------------------------------- 00377 /// @brief xor two vectors 00378 /// @param buf u8* inout stream 00379 /// @param iv u8* in stream to XOR 00380 /// @retval void 00381 //====================================================================== 00382 _public void kal_xor(u8* buf, u8* iv, u16 len); 00383 00384 //====================================================================== 00385 // kal_sort 00386 //---------------------------------------------------------------------- 00387 /// @brief Sort u16 buffer from smaller to bigger error 00388 /// @param inout u16* inout buffer 00389 /// @param ref u16 comparison reference 00390 /// @param len u8 buffer length 00391 /// @return void 00392 //====================================================================== 00393 _public void kal_sort(u16* inout, u16 ref, u8 len); 00394 00395 #endif // __KAL_MATH_H__
Generated on Tue Jul 12 2022 13:52:23 by
1.7.2