WizziLab / modem_ref

Exportable version of WizziLab's modem driver.

Dependents:   modem_ref_helper

src/kal_math.cpp@67:e458db8402dc, 2021-10-29 (annotated)

Committer:
marin_wizzi
Date:
Fri Oct 29 13:54:43 2021 +0000
Revision:
67:e458db8402dc
Parent:
56:67e3d9608403 
had to be commited for Scanner test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jeej 56:67e3d9608403 1 /// @copyright
Jeej 56:67e3d9608403 2 /// ========================================================================={{{
Jeej 56:67e3d9608403 3 /// Copyright (c) 2013 WizziLab /
Jeej 56:67e3d9608403 4 /// All rights reserved /
Jeej 56:67e3d9608403 5 /// /
Jeej 56:67e3d9608403 6 /// IMPORTANT: This Software may not be modified, copied or distributed unless /
Jeej 56:67e3d9608403 7 /// embedded on a WizziLab product. Other than for the foregoing purpose, this /
Jeej 56:67e3d9608403 8 /// Software and/or its documentation may not be used, reproduced, copied, /
Jeej 56:67e3d9608403 9 /// prepared derivative works of, modified, performed, distributed, displayed /
Jeej 56:67e3d9608403 10 /// or sold for any purpose. For the sole purpose of embedding this Software /
Jeej 56:67e3d9608403 11 /// on a WizziLab product, copy, modification and distribution of this /
Jeej 56:67e3d9608403 12 /// Software is granted provided that the following conditions are respected: /
Jeej 56:67e3d9608403 13 /// /
Jeej 56:67e3d9608403 14 /// * Redistributions of source code must retain the above copyright notice, /
Jeej 56:67e3d9608403 15 /// this list of conditions and the following disclaimer /
Jeej 56:67e3d9608403 16 /// /
Jeej 56:67e3d9608403 17 /// * Redistributions in binary form must reproduce the above copyright /
Jeej 56:67e3d9608403 18 /// notice, this list of conditions and the following disclaimer in the /
Jeej 56:67e3d9608403 19 /// documentation and/or other materials provided with the distribution. /
Jeej 56:67e3d9608403 20 /// /
Jeej 56:67e3d9608403 21 /// * The name of WizziLab can not be used to endorse or promote products /
Jeej 56:67e3d9608403 22 /// derived from this software without specific prior written permission. /
Jeej 56:67e3d9608403 23 /// /
Jeej 56:67e3d9608403 24 /// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS /
Jeej 56:67e3d9608403 25 /// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED /
Jeej 56:67e3d9608403 26 /// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR /
Jeej 56:67e3d9608403 27 /// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR /
Jeej 56:67e3d9608403 28 /// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, /
Jeej 56:67e3d9608403 29 /// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, /
Jeej 56:67e3d9608403 30 /// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, /
Jeej 56:67e3d9608403 31 /// OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY /
Jeej 56:67e3d9608403 32 /// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING /
Jeej 56:67e3d9608403 33 /// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS /
Jeej 56:67e3d9608403 34 /// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. /
Jeej 56:67e3d9608403 35 /// WIZZILAB HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, /
Jeej 56:67e3d9608403 36 /// ENHANCEMENTS OR MODIFICATIONS. /
Jeej 56:67e3d9608403 37 /// /
Jeej 56:67e3d9608403 38 /// Should you have any questions regarding your right to use this Software, /
Jeej 56:67e3d9608403 39 /// contact WizziLab at www.wizzilab.com. /
Jeej 56:67e3d9608403 40 /// /
Jeej 56:67e3d9608403 41 /// =========================================================================}}}
Jeej 56:67e3d9608403 42 /// @endcopyright
Jeej 56:67e3d9608403 43
Jeej 56:67e3d9608403 44 #include "hal_types.h"
Jeej 56:67e3d9608403 45 #include "kal_math.h"
Jeej 56:67e3d9608403 46 #include "WizziDebug.h"
Jeej 56:67e3d9608403 47
Jeej 56:67e3d9608403 48 #ifndef KAL_MALLOC
Jeej 56:67e3d9608403 49 #define KAL_MALLOC MALLOC
Jeej 56:67e3d9608403 50 #endif
Jeej 56:67e3d9608403 51
Jeej 56:67e3d9608403 52 #ifndef KAL_FREE
Jeej 56:67e3d9608403 53 #define KAL_FREE FREE
Jeej 56:67e3d9608403 54 #endif
Jeej 56:67e3d9608403 55
Jeej 56:67e3d9608403 56 // =======================================================================
Jeej 56:67e3d9608403 57 // kal_log2
Jeej 56:67e3d9608403 58 // -----------------------------------------------------------------------
Jeej 56:67e3d9608403 59 /// @brief Fast log2 computation
Jeej 56:67e3d9608403 60 /// @param in u32 operand
Jeej 56:67e3d9608403 61 /// @retval u8 log2 of the operand
Jeej 56:67e3d9608403 62 // =======================================================================
Jeej 56:67e3d9608403 63 _public u8 kal_log2(u32 in)
Jeej 56:67e3d9608403 64 {
Jeej 56:67e3d9608403 65 if (in < 2)
Jeej 56:67e3d9608403 66 {
Jeej 56:67e3d9608403 67 // no negative return
Jeej 56:67e3d9608403 68 return 0;
Jeej 56:67e3d9608403 69 }
Jeej 56:67e3d9608403 70 else if (MAX_U32 == in)
Jeej 56:67e3d9608403 71 {
Jeej 56:67e3d9608403 72 // log2(2^32)
Jeej 56:67e3d9608403 73 return 32;
Jeej 56:67e3d9608403 74 }
Jeej 56:67e3d9608403 75 else
Jeej 56:67e3d9608403 76 {
Jeej 56:67e3d9608403 77 u32 x;
Jeej 56:67e3d9608403 78 u8 norm;
Jeej 56:67e3d9608403 79 for (x = in, norm = 31; x < 0x80000000L; norm--, x <<= 1);
Jeej 56:67e3d9608403 80 return norm;
Jeej 56:67e3d9608403 81 }
Jeej 56:67e3d9608403 82 }
Jeej 56:67e3d9608403 83
Jeej 56:67e3d9608403 84 // =======================================================================
Jeej 56:67e3d9608403 85 // kal_sqrt
Jeej 56:67e3d9608403 86 // -----------------------------------------------------------------------
Jeej 56:67e3d9608403 87 /// @brief Fixed point square root
Jeej 56:67e3d9608403 88 /// @param x u16 input fixed point value
Jeej 56:67e3d9608403 89 /// @param format u8 fixed point format (shift)
Jeej 56:67e3d9608403 90 /// @retval u16 square root of the input in the given format
Jeej 56:67e3d9608403 91 // =======================================================================
Jeej 56:67e3d9608403 92 _public u16 kal_sqrt(u16 x, u8 format)
Jeej 56:67e3d9608403 93 {
Jeej 56:67e3d9608403 94 #define KAL_SQRT_ITER (5)
Jeej 56:67e3d9608403 95 u32 r, s;
Jeej 56:67e3d9608403 96 u8 i;
Jeej 56:67e3d9608403 97
Jeej 56:67e3d9608403 98 if (!x)
Jeej 56:67e3d9608403 99 {
Jeej 56:67e3d9608403 100 // sqrt(0)
Jeej 56:67e3d9608403 101 return 0;
Jeej 56:67e3d9608403 102 }
Jeej 56:67e3d9608403 103 else
Jeej 56:67e3d9608403 104 {
Jeej 56:67e3d9608403 105 // value to sqrt
Jeej 56:67e3d9608403 106 s = x << format;
Jeej 56:67e3d9608403 107
Jeej 56:67e3d9608403 108 // start iteration with good sqrt estimator
Jeej 56:67e3d9608403 109 r = s >> ((kal_log2(x) + format)/2);
Jeej 56:67e3d9608403 110
Jeej 56:67e3d9608403 111 // For better precision, increase number of iterations
Jeej 56:67e3d9608403 112 for (i = 0; (r != 0) && (i < KAL_SQRT_ITER); ++i)
Jeej 56:67e3d9608403 113 {
Jeej 56:67e3d9608403 114 r = (s/r + r)/2;
Jeej 56:67e3d9608403 115 }
Jeej 56:67e3d9608403 116
Jeej 56:67e3d9608403 117 return (u16)r;
Jeej 56:67e3d9608403 118 }
Jeej 56:67e3d9608403 119 }
Jeej 56:67e3d9608403 120
Jeej 56:67e3d9608403 121 // =======================================================================
Jeej 56:67e3d9608403 122 // kal_sqrt32
Jeej 56:67e3d9608403 123 // -----------------------------------------------------------------------
Jeej 56:67e3d9608403 124 /// @brief Integer square root
Jeej 56:67e3d9608403 125 /// @param x u32 input fixed point value
Jeej 56:67e3d9608403 126 /// @retval u16 square root of the input
Jeej 56:67e3d9608403 127 // =======================================================================
Jeej 56:67e3d9608403 128 _public u16 kal_sqrt32(u32 x)
Jeej 56:67e3d9608403 129 {
Jeej 56:67e3d9608403 130 u32 r, s;
Jeej 56:67e3d9608403 131 u8 i;
Jeej 56:67e3d9608403 132
Jeej 56:67e3d9608403 133 if (!x)
Jeej 56:67e3d9608403 134 {
Jeej 56:67e3d9608403 135 // sqrt(0)
Jeej 56:67e3d9608403 136 return 0;
Jeej 56:67e3d9608403 137 }
Jeej 56:67e3d9608403 138 else
Jeej 56:67e3d9608403 139 {
Jeej 56:67e3d9608403 140 #if 1
Jeej 56:67e3d9608403 141 // value to sqrt
Jeej 56:67e3d9608403 142 s = x;
Jeej 56:67e3d9608403 143
Jeej 56:67e3d9608403 144 // start iteration with good sqrt estimator
Jeej 56:67e3d9608403 145 r = s >> (kal_log2(x)/2);
Jeej 56:67e3d9608403 146
Jeej 56:67e3d9608403 147 // For better precision, increase number of iterations
Jeej 56:67e3d9608403 148 for (i = 0; (r != 0) && (i < KAL_SQRT_ITER); ++i)
Jeej 56:67e3d9608403 149 {
Jeej 56:67e3d9608403 150 r = (s/r + r)/2;
Jeej 56:67e3d9608403 151 }
Jeej 56:67e3d9608403 152 #else
Jeej 56:67e3d9608403 153 s = 1 << 30;
Jeej 56:67e3d9608403 154 r = 0;
Jeej 56:67e3d9608403 155
Jeej 56:67e3d9608403 156 while (s > x)
Jeej 56:67e3d9608403 157 s >>= 2;
Jeej 56:67e3d9608403 158
Jeej 56:67e3d9608403 159 while (s != 0) {
Jeej 56:67e3d9608403 160 if (x >= r + s) {
Jeej 56:67e3d9608403 161 x -= r + s;
Jeej 56:67e3d9608403 162 r = (r >> 1) + s;
Jeej 56:67e3d9608403 163 }
Jeej 56:67e3d9608403 164
Jeej 56:67e3d9608403 165 else
Jeej 56:67e3d9608403 166 r >>= 1;
Jeej 56:67e3d9608403 167 s >>= 2;
Jeej 56:67e3d9608403 168 }
Jeej 56:67e3d9608403 169
Jeej 56:67e3d9608403 170 #endif
Jeej 56:67e3d9608403 171 return (u16)r;
Jeej 56:67e3d9608403 172 }
Jeej 56:67e3d9608403 173 }
Jeej 56:67e3d9608403 174
Jeej 56:67e3d9608403 175 // =======================================================================
Jeej 56:67e3d9608403 176 // kal_div
Jeej 56:67e3d9608403 177 // -----------------------------------------------------------------------
Jeej 56:67e3d9608403 178 /// @brief Fixed point division of two s16 values
Jeej 56:67e3d9608403 179 /// @param nom s16 Numerator
Jeej 56:67e3d9608403 180 /// @param denom s16 Denominator
Jeej 56:67e3d9608403 181 /// @param format u8 fixed point format (shift)
Jeej 56:67e3d9608403 182 /// @retval s16 division result in the given format
Jeej 56:67e3d9608403 183 // =======================================================================
Jeej 56:67e3d9608403 184 _public s16 kal_div(s16 num, s16 denom, u8 format)
Jeej 56:67e3d9608403 185 {
Jeej 56:67e3d9608403 186 if (!denom)
Jeej 56:67e3d9608403 187 {
Jeej 56:67e3d9608403 188 return ((num < 0) ? MIN_S16 : MAX_S16);
Jeej 56:67e3d9608403 189 }
Jeej 56:67e3d9608403 190 else
Jeej 56:67e3d9608403 191 {
Jeej 56:67e3d9608403 192 s16 sign = (num ^ denom) >> 15;
Jeej 56:67e3d9608403 193 u32 n = kal_abs_s32(num);
Jeej 56:67e3d9608403 194 u32 d = kal_abs_s32(denom);
Jeej 56:67e3d9608403 195 s8 rescale = 15 - format;
Jeej 56:67e3d9608403 196
Jeej 56:67e3d9608403 197 while ((d <= n) && (rescale >= 0))
Jeej 56:67e3d9608403 198 {
Jeej 56:67e3d9608403 199 rescale--;
Jeej 56:67e3d9608403 200 d <<= 1;
Jeej 56:67e3d9608403 201 }
Jeej 56:67e3d9608403 202
Jeej 56:67e3d9608403 203 if (rescale < 0)
Jeej 56:67e3d9608403 204 {
Jeej 56:67e3d9608403 205 return ((sign) ? MIN_S16 : MAX_S16);
Jeej 56:67e3d9608403 206 }
Jeej 56:67e3d9608403 207 else
Jeej 56:67e3d9608403 208 {
Jeej 56:67e3d9608403 209 u8 i;
Jeej 56:67e3d9608403 210 u16 res;
Jeej 56:67e3d9608403 211
Jeej 56:67e3d9608403 212 for (res = 0, i = 0; i < 15; ++i)
Jeej 56:67e3d9608403 213 {
Jeej 56:67e3d9608403 214 res <<= 1;
Jeej 56:67e3d9608403 215 n <<= 1;
Jeej 56:67e3d9608403 216 if (n >= d)
Jeej 56:67e3d9608403 217 {
Jeej 56:67e3d9608403 218 n -= d;
Jeej 56:67e3d9608403 219 res++;
Jeej 56:67e3d9608403 220 }
Jeej 56:67e3d9608403 221 }
Jeej 56:67e3d9608403 222 res >>= rescale;
Jeej 56:67e3d9608403 223 return ((sign) ? -res : res);
Jeej 56:67e3d9608403 224 }
Jeej 56:67e3d9608403 225 }
Jeej 56:67e3d9608403 226 }
Jeej 56:67e3d9608403 227
Jeej 56:67e3d9608403 228 // =======================================================================
Jeej 56:67e3d9608403 229 // kal_div_u32
Jeej 56:67e3d9608403 230 // -----------------------------------------------------------------------
Jeej 56:67e3d9608403 231 /// @brief Fixed point division of two u32 values
Jeej 56:67e3d9608403 232 /// @param n u32 Numerator
Jeej 56:67e3d9608403 233 /// @param d u32 Denominator
Jeej 56:67e3d9608403 234 /// @param format u8 fixed point format (shift)
Jeej 56:67e3d9608403 235 /// @retval u32 division result in the given format
Jeej 56:67e3d9608403 236 // =======================================================================
Jeej 56:67e3d9608403 237 _public u32 kal_div_u32(u32 n, u32 d, u8 format)
Jeej 56:67e3d9608403 238 {
Jeej 56:67e3d9608403 239 if (!d)
Jeej 56:67e3d9608403 240 {
Jeej 56:67e3d9608403 241 return MAX_U32;
Jeej 56:67e3d9608403 242 }
Jeej 56:67e3d9608403 243 else
Jeej 56:67e3d9608403 244 {
Jeej 56:67e3d9608403 245 s8 rescale = 31 - format;
Jeej 56:67e3d9608403 246
Jeej 56:67e3d9608403 247 if (n > MAX_S32)
Jeej 56:67e3d9608403 248 {
Jeej 56:67e3d9608403 249 rescale--;
Jeej 56:67e3d9608403 250 n >>= 1;
Jeej 56:67e3d9608403 251 }
Jeej 56:67e3d9608403 252
Jeej 56:67e3d9608403 253 // avoid never true comparison
Jeej 56:67e3d9608403 254 // in sequence n <<= 1; if (n >= d)...
Jeej 56:67e3d9608403 255 // when d > MAX_S32;
Jeej 56:67e3d9608403 256 if (d > MAX_S32)
Jeej 56:67e3d9608403 257 {
Jeej 56:67e3d9608403 258 n >>= 1;
Jeej 56:67e3d9608403 259 d >>= 1;
Jeej 56:67e3d9608403 260 }
Jeej 56:67e3d9608403 261
Jeej 56:67e3d9608403 262 while ((d <= n) && (rescale >= 0))
Jeej 56:67e3d9608403 263 {
Jeej 56:67e3d9608403 264 rescale--;
Jeej 56:67e3d9608403 265 d <<= 1;
Jeej 56:67e3d9608403 266 }
Jeej 56:67e3d9608403 267
Jeej 56:67e3d9608403 268 if (rescale < 0)
Jeej 56:67e3d9608403 269 {
Jeej 56:67e3d9608403 270 return MAX_U32;
Jeej 56:67e3d9608403 271 }
Jeej 56:67e3d9608403 272 else
Jeej 56:67e3d9608403 273 {
Jeej 56:67e3d9608403 274 u8 i;
Jeej 56:67e3d9608403 275 u32 res;
Jeej 56:67e3d9608403 276
Jeej 56:67e3d9608403 277 for (res = 0, i = 0; i < 31; ++i)
Jeej 56:67e3d9608403 278 {
Jeej 56:67e3d9608403 279 res <<= 1;
Jeej 56:67e3d9608403 280 n <<= 1;
Jeej 56:67e3d9608403 281 if (n >= d)
Jeej 56:67e3d9608403 282 {
Jeej 56:67e3d9608403 283 n -= d;
Jeej 56:67e3d9608403 284 res++;
Jeej 56:67e3d9608403 285 }
Jeej 56:67e3d9608403 286 }
Jeej 56:67e3d9608403 287 res >>= rescale;
Jeej 56:67e3d9608403 288 return res;
Jeej 56:67e3d9608403 289 }
Jeej 56:67e3d9608403 290 }
Jeej 56:67e3d9608403 291 }
Jeej 56:67e3d9608403 292
Jeej 56:67e3d9608403 293 // =======================================================================
Jeej 56:67e3d9608403 294 // sin(x), x in [0°, 90°], in Q15
Jeej 56:67e3d9608403 295 // =======================================================================
Jeej 56:67e3d9608403 296 const u16 k_sin[] =
Jeej 56:67e3d9608403 297 {
Jeej 56:67e3d9608403 298 0, 572, 1144, 1715, 2286, 2856, 3425, 3993, 4560, 5126,
Jeej 56:67e3d9608403 299 5690, 6252, 6813, 7371, 7927, 8481, 9032, 9580, 10126, 10668,
Jeej 56:67e3d9608403 300 11207, 11743, 12275, 12803, 13328, 13848, 14365, 14876, 15384, 15886,
Jeej 56:67e3d9608403 301 16384, 16877, 17364, 17847, 18324, 18795, 19261, 19720, 20174, 20622,
Jeej 56:67e3d9608403 302 21063, 21498, 21926, 22348, 22763, 23170, 23571, 23965, 24351, 24730,
Jeej 56:67e3d9608403 303 25102, 25466, 25822, 26170, 26510, 26842, 27166, 27482, 27789, 28088,
Jeej 56:67e3d9608403 304 28378, 28660, 28932, 29197, 29452, 29698, 29935, 30163, 30382, 30592,
Jeej 56:67e3d9608403 305 30792, 30983, 31164, 31336, 31499, 31651, 31795, 31928, 32052, 32166,
Jeej 56:67e3d9608403 306 32270, 32365, 32449, 32524, 32588, 32643, 32688, 32723, 32748, 32763,
Jeej 56:67e3d9608403 307 32767
Jeej 56:67e3d9608403 308 };
Jeej 56:67e3d9608403 309
Jeej 56:67e3d9608403 310 // =======================================================================
Jeej 56:67e3d9608403 311 // kal_complex_arg
Jeej 56:67e3d9608403 312 // -----------------------------------------------------------------------
Jeej 56:67e3d9608403 313 /// @brief Compute the argument of a complex number
Jeej 56:67e3d9608403 314 /// @param in u32 operand
Jeej 56:67e3d9608403 315 /// @retval u16 angle in degrees [0, 360[
Jeej 56:67e3d9608403 316 // =======================================================================
Jeej 56:67e3d9608403 317 _public u16 kal_complex_arg(complex_t in)
Jeej 56:67e3d9608403 318 {
Jeej 56:67e3d9608403 319 u16 angle = 0;
Jeej 56:67e3d9608403 320 if ((in.I) || (in.Q))
Jeej 56:67e3d9608403 321 {
Jeej 56:67e3d9608403 322 u16 min = 0, max = 90, mid, sine;
Jeej 56:67e3d9608403 323
Jeej 56:67e3d9608403 324 // compute the sine in.i / sqrt(in.i^2 + in.q^2)
Jeej 56:67e3d9608403 325 u16 norm = kal_sqrt32(in.I * in.I + in.Q * in.Q);
Jeej 56:67e3d9608403 326 sine = (norm) ? (u16)(((u32)kal_abs_s32(in.Q << 15) + norm/2) / norm) : 0;
Jeej 56:67e3d9608403 327
Jeej 56:67e3d9608403 328 // find angle from LUT (1° precision) using dichotomy
Jeej 56:67e3d9608403 329 while ((max - min) > 1)
Jeej 56:67e3d9608403 330 {
Jeej 56:67e3d9608403 331 angle = (max + min) / 2;
Jeej 56:67e3d9608403 332 if (sine < k_sin[angle])
Jeej 56:67e3d9608403 333 {
Jeej 56:67e3d9608403 334 max = angle;
Jeej 56:67e3d9608403 335 }
Jeej 56:67e3d9608403 336 else
Jeej 56:67e3d9608403 337 {
Jeej 56:67e3d9608403 338 min = angle;
Jeej 56:67e3d9608403 339 }
Jeej 56:67e3d9608403 340 }
Jeej 56:67e3d9608403 341
Jeej 56:67e3d9608403 342 min = kal_abs_s16(k_sin[angle-1] - sine);
Jeej 56:67e3d9608403 343 mid = kal_abs_s16(k_sin[angle ] - sine);
Jeej 56:67e3d9608403 344 max = kal_abs_s16(k_sin[angle+1] - sine);
Jeej 56:67e3d9608403 345
Jeej 56:67e3d9608403 346 // best approx
Jeej 56:67e3d9608403 347 if ((min < max) && (min < mid))
Jeej 56:67e3d9608403 348 {
Jeej 56:67e3d9608403 349 angle--;
Jeej 56:67e3d9608403 350 }
Jeej 56:67e3d9608403 351 else if ((max > min) && (max > mid))
Jeej 56:67e3d9608403 352 {
Jeej 56:67e3d9608403 353 angle++;
Jeej 56:67e3d9608403 354 }
Jeej 56:67e3d9608403 355
Jeej 56:67e3d9608403 356 // quadrant
Jeej 56:67e3d9608403 357 if (in.I < 0)
Jeej 56:67e3d9608403 358 {
Jeej 56:67e3d9608403 359 angle = (in.Q < 0) ? angle + 180 : 180 - angle;
Jeej 56:67e3d9608403 360 }
Jeej 56:67e3d9608403 361 else
Jeej 56:67e3d9608403 362 {
Jeej 56:67e3d9608403 363 angle = (in.Q < 0) ? 360 - angle : angle;
Jeej 56:67e3d9608403 364 }
Jeej 56:67e3d9608403 365 }
Jeej 56:67e3d9608403 366
Jeej 56:67e3d9608403 367 return angle;
Jeej 56:67e3d9608403 368 }
Jeej 56:67e3d9608403 369
Jeej 56:67e3d9608403 370 // =======================================================================
Jeej 56:67e3d9608403 371 // kal_sin
Jeej 56:67e3d9608403 372 // -----------------------------------------------------------------------
Jeej 56:67e3d9608403 373 /// @brief sin(a) in Q15
Jeej 56:67e3d9608403 374 /// @param a u16 angle in degrees [0, 360[
Jeej 56:67e3d9608403 375 /// @retval s16 sin(a) in Q15
Jeej 56:67e3d9608403 376 // =======================================================================
Jeej 56:67e3d9608403 377 _public u16 kal_sin(u16 a)
Jeej 56:67e3d9608403 378 {
Jeej 56:67e3d9608403 379 if (a < 90)
Jeej 56:67e3d9608403 380 {
Jeej 56:67e3d9608403 381 return k_sin[a];
Jeej 56:67e3d9608403 382 }
Jeej 56:67e3d9608403 383 else if (a < 180)
Jeej 56:67e3d9608403 384 {
Jeej 56:67e3d9608403 385 return k_sin[180 - a];
Jeej 56:67e3d9608403 386 }
Jeej 56:67e3d9608403 387 else if (a < 270)
Jeej 56:67e3d9608403 388 {
Jeej 56:67e3d9608403 389 return -k_sin[a - 180];
Jeej 56:67e3d9608403 390 }
Jeej 56:67e3d9608403 391 else
Jeej 56:67e3d9608403 392 {
Jeej 56:67e3d9608403 393 return -k_sin[360 - a];
Jeej 56:67e3d9608403 394 }
Jeej 56:67e3d9608403 395 }
Jeej 56:67e3d9608403 396
Jeej 56:67e3d9608403 397 // =======================================================================
Jeej 56:67e3d9608403 398 // kal_add
Jeej 56:67e3d9608403 399 // -----------------------------------------------------------------------
Jeej 56:67e3d9608403 400 /// @brief Add B to A and store in A
Jeej 56:67e3d9608403 401 /// @param a s16* pointer to the d-dimensional vector A
Jeej 56:67e3d9608403 402 /// @param b s16* pointer to the d-dimensional vector B
Jeej 56:67e3d9608403 403 /// @param d u8 number of dimensions of the buffer
Jeej 56:67e3d9608403 404 /// @retval void
Jeej 56:67e3d9608403 405 // =======================================================================
Jeej 56:67e3d9608403 406 _public void kal_add(s16* a, s16* b, u8 d)
Jeej 56:67e3d9608403 407 {
Jeej 56:67e3d9608403 408 u8 i;
Jeej 56:67e3d9608403 409 for (i = 0; i < d; ++i)
Jeej 56:67e3d9608403 410 {
Jeej 56:67e3d9608403 411 s16 temp = *a;
Jeej 56:67e3d9608403 412 *a++ = temp + *b++;
Jeej 56:67e3d9608403 413 }
Jeej 56:67e3d9608403 414 }
Jeej 56:67e3d9608403 415
Jeej 56:67e3d9608403 416 // =======================================================================
Jeej 56:67e3d9608403 417 // kal_sub
Jeej 56:67e3d9608403 418 // -----------------------------------------------------------------------
Jeej 56:67e3d9608403 419 /// @brief Subtract B from A and store in A
Jeej 56:67e3d9608403 420 /// @param a s16* pointer to the d-dimensional vector A
Jeej 56:67e3d9608403 421 /// @param b s16* pointer to the d-dimensional vector B
Jeej 56:67e3d9608403 422 /// @param d u8 number of dimensions of the buffer
Jeej 56:67e3d9608403 423 /// @retval void
Jeej 56:67e3d9608403 424 // =======================================================================
Jeej 56:67e3d9608403 425 _public void kal_sub(s16* a, s16* b, u8 d)
Jeej 56:67e3d9608403 426 {
Jeej 56:67e3d9608403 427 u8 i;
Jeej 56:67e3d9608403 428 for (i = 0; i < d; ++i)
Jeej 56:67e3d9608403 429 {
Jeej 56:67e3d9608403 430 s16 temp = *a;
Jeej 56:67e3d9608403 431 *a++ = temp - *b++;
Jeej 56:67e3d9608403 432 }
Jeej 56:67e3d9608403 433 }
Jeej 56:67e3d9608403 434
Jeej 56:67e3d9608403 435 // =======================================================================
Jeej 56:67e3d9608403 436 // kal_mean
Jeej 56:67e3d9608403 437 // -----------------------------------------------------------------------
Jeej 56:67e3d9608403 438 /// @brief Get mean of data in a d-dimensional buffer
Jeej 56:67e3d9608403 439 /// @param in s16* input circular buffer
Jeej 56:67e3d9608403 440 /// @param out s16* pointer to the d-dimensional mean result
Jeej 56:67e3d9608403 441 /// @param len u8 length of the buffer
Jeej 56:67e3d9608403 442 /// @param d u8 number of dimensions of the buffer
Jeej 56:67e3d9608403 443 /// @retval void
Jeej 56:67e3d9608403 444 // =======================================================================
Jeej 56:67e3d9608403 445 _public void kal_mean(s16* in, s16* out, u8 len, u8 d)
Jeej 56:67e3d9608403 446 {
Jeej 56:67e3d9608403 447 u8 i;
Jeej 56:67e3d9608403 448 for (i = 0; i < d; ++i)
Jeej 56:67e3d9608403 449 {
Jeej 56:67e3d9608403 450 u8 j;
Jeej 56:67e3d9608403 451 s16* p = in++;
Jeej 56:67e3d9608403 452 s32 mean = 0;
Jeej 56:67e3d9608403 453
Jeej 56:67e3d9608403 454 for (j = 0; j < len; ++j)
Jeej 56:67e3d9608403 455 {
Jeej 56:67e3d9608403 456 mean += *p;
Jeej 56:67e3d9608403 457 p += d;
Jeej 56:67e3d9608403 458 }
Jeej 56:67e3d9608403 459
Jeej 56:67e3d9608403 460 mean = (mean + len/2) / len;
Jeej 56:67e3d9608403 461 *out++ = KAL_SAT_S16(mean);
Jeej 56:67e3d9608403 462 }
Jeej 56:67e3d9608403 463 }
Jeej 56:67e3d9608403 464
Jeej 56:67e3d9608403 465 // =======================================================================
Jeej 56:67e3d9608403 466 // kal_var
Jeej 56:67e3d9608403 467 // -----------------------------------------------------------------------
Jeej 56:67e3d9608403 468 /// @brief Get the combined variance of data in a d-dimensional buffer
Jeej 56:67e3d9608403 469 /// @param in s16* input circular buffer
Jeej 56:67e3d9608403 470 /// @param mean s16* pointer to the d-dimensional mean
Jeej 56:67e3d9608403 471 /// @param len u8 length of the buffer
Jeej 56:67e3d9608403 472 /// @param d u8 number of dimensions of the buffer
Jeej 56:67e3d9608403 473 /// @retval u32 variance
Jeej 56:67e3d9608403 474 // =======================================================================
Jeej 56:67e3d9608403 475 _public u32 kal_var(s16* in, s16* mean, u8 len, u8 d)
Jeej 56:67e3d9608403 476 {
Jeej 56:67e3d9608403 477 u8 i,j;
Jeej 56:67e3d9608403 478 u32 var = 0;
Jeej 56:67e3d9608403 479 for (i = 0; i < d; ++i)
Jeej 56:67e3d9608403 480 {
Jeej 56:67e3d9608403 481 // get var
Jeej 56:67e3d9608403 482 s16* p = in;
Jeej 56:67e3d9608403 483 for (j = 0; j < len; ++j)
Jeej 56:67e3d9608403 484 {
Jeej 56:67e3d9608403 485 s16 s = *p - *mean;
Jeej 56:67e3d9608403 486 var += KAL_MUL(s,s);
Jeej 56:67e3d9608403 487 p += d;
Jeej 56:67e3d9608403 488
Jeej 56:67e3d9608403 489 // avoid overflows
Jeej 56:67e3d9608403 490 if (var > MAX_S32)
Jeej 56:67e3d9608403 491 {
Jeej 56:67e3d9608403 492 return MAX_U32;
Jeej 56:67e3d9608403 493 }
Jeej 56:67e3d9608403 494 }
Jeej 56:67e3d9608403 495 in++;
Jeej 56:67e3d9608403 496 mean++;
Jeej 56:67e3d9608403 497 }
Jeej 56:67e3d9608403 498 var = (var + len/2) / len;
Jeej 56:67e3d9608403 499 return var;
Jeej 56:67e3d9608403 500 }
Jeej 56:67e3d9608403 501
Jeej 56:67e3d9608403 502 // =======================================================================
Jeej 56:67e3d9608403 503 // kal_dev
Jeej 56:67e3d9608403 504 // -----------------------------------------------------------------------
Jeej 56:67e3d9608403 505 /// @brief Get the deviation per axis in a d-dimensional buffer
Jeej 56:67e3d9608403 506 /// @param buf s16* input circular buffer
Jeej 56:67e3d9608403 507 /// @param inout s16* [in] pointer to the d-dimensional mean
Jeej 56:67e3d9608403 508 /// [out] pointer to the d-dimensional dev
Jeej 56:67e3d9608403 509 /// @param len u8 length of the buffer
Jeej 56:67e3d9608403 510 /// @param d u8 number of dimensions of the buffer
Jeej 56:67e3d9608403 511 /// @retval void
Jeej 56:67e3d9608403 512 // =======================================================================
Jeej 56:67e3d9608403 513 _public void kal_dev(s16* buf, s16* inout, u8 len, u8 d)
Jeej 56:67e3d9608403 514 {
Jeej 56:67e3d9608403 515 u8 i, j;
Jeej 56:67e3d9608403 516 u32 var;
Jeej 56:67e3d9608403 517 s32 sqrt;
Jeej 56:67e3d9608403 518 s16* p, mean;
Jeej 56:67e3d9608403 519
Jeej 56:67e3d9608403 520 for (i = 0; i < d; ++i)
Jeej 56:67e3d9608403 521 {
Jeej 56:67e3d9608403 522 var = 0;
Jeej 56:67e3d9608403 523 p = buf++;
Jeej 56:67e3d9608403 524 mean = *inout;
Jeej 56:67e3d9608403 525
Jeej 56:67e3d9608403 526 for (j = 0; j < len; ++j)
Jeej 56:67e3d9608403 527 {
Jeej 56:67e3d9608403 528 s16 s = *p - mean;
Jeej 56:67e3d9608403 529 var += KAL_MUL(s,s);
Jeej 56:67e3d9608403 530 p += d;
Jeej 56:67e3d9608403 531
Jeej 56:67e3d9608403 532 // avoid overflows
Jeej 56:67e3d9608403 533 if (var > MAX_S32)
Jeej 56:67e3d9608403 534 {
Jeej 56:67e3d9608403 535 var = MAX_U32;
Jeej 56:67e3d9608403 536 break;
Jeej 56:67e3d9608403 537 }
Jeej 56:67e3d9608403 538 }
Jeej 56:67e3d9608403 539 var = KAL_DIV_INT(var,len);
Jeej 56:67e3d9608403 540 sqrt = kal_sqrt32(var);
Jeej 56:67e3d9608403 541 *inout++ = KAL_SAT_S16(sqrt);
Jeej 56:67e3d9608403 542 }
Jeej 56:67e3d9608403 543 }
Jeej 56:67e3d9608403 544
Jeej 56:67e3d9608403 545 // =======================================================================
Jeej 56:67e3d9608403 546 // kal_norm
Jeej 56:67e3d9608403 547 // -----------------------------------------------------------------------
Jeej 56:67e3d9608403 548 /// @brief Get norm of an d-dimensional vector
Jeej 56:67e3d9608403 549 /// @param in s16* d-dimensional vector
Jeej 56:67e3d9608403 550 /// @param d u8 number of dimensions of the vector
Jeej 56:67e3d9608403 551 /// @retval u32 norm of the vector
Jeej 56:67e3d9608403 552 // =======================================================================
Jeej 56:67e3d9608403 553 _public u32 kal_norm(s16* in, u8 d)
Jeej 56:67e3d9608403 554 {
Jeej 56:67e3d9608403 555 u8 i;
Jeej 56:67e3d9608403 556 u32 norm = 0;
Jeej 56:67e3d9608403 557 for (i = 0; i < d; ++i)
Jeej 56:67e3d9608403 558 {
Jeej 56:67e3d9608403 559 s16 s = *in++;
Jeej 56:67e3d9608403 560 norm += KAL_MUL(s,s);
Jeej 56:67e3d9608403 561 }
Jeej 56:67e3d9608403 562 return norm;
Jeej 56:67e3d9608403 563 }
Jeej 56:67e3d9608403 564
Jeej 56:67e3d9608403 565 // =======================================================================
Jeej 56:67e3d9608403 566 // kal_dist
Jeej 56:67e3d9608403 567 // -----------------------------------------------------------------------
Jeej 56:67e3d9608403 568 /// @brief Get distance between two d-dimensional vectors
Jeej 56:67e3d9608403 569 /// @param a s16* first d-dimensional vector
Jeej 56:67e3d9608403 570 /// @param b s16* second d-dimensional vector
Jeej 56:67e3d9608403 571 /// @param d u8 number of dimensions of the vector
Jeej 56:67e3d9608403 572 /// @retval u32 distance (norm of the difference)
Jeej 56:67e3d9608403 573 // =======================================================================
Jeej 56:67e3d9608403 574 _public u32 kal_dist(s16* a, s16* b, u8 d)
Jeej 56:67e3d9608403 575 {
Jeej 56:67e3d9608403 576 u8 i;
Jeej 56:67e3d9608403 577 u32 norm = 0;
Jeej 56:67e3d9608403 578 for (i = 0; i < d; ++i)
Jeej 56:67e3d9608403 579 {
Jeej 56:67e3d9608403 580 s32 s_long = (*a++) - (*b++);
Jeej 56:67e3d9608403 581 s16 s = KAL_SAT_S16(s_long);
Jeej 56:67e3d9608403 582 norm += KAL_MUL(s,s);
Jeej 56:67e3d9608403 583 }
Jeej 56:67e3d9608403 584 return norm;
Jeej 56:67e3d9608403 585 }
Jeej 56:67e3d9608403 586
Jeej 56:67e3d9608403 587 //======================================================================
Jeej 56:67e3d9608403 588 // kal_circ_buf_alloc
Jeej 56:67e3d9608403 589 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 590 /// @brief Allocate and init circular buffer
Jeej 56:67e3d9608403 591 /// @param len u8 Number of d-dimensional vectors in the buffer
Jeej 56:67e3d9608403 592 /// @param dim u8 Number of dimensions of the buffer
Jeej 56:67e3d9608403 593 /// @return kal_circ_buf_t* Pointer to the circular buffer structure
Jeej 56:67e3d9608403 594 //======================================================================
Jeej 56:67e3d9608403 595 _public kal_circ_buf_t* kal_circ_buf_alloc(u8 len, u8 dim)
Jeej 56:67e3d9608403 596 {
Jeej 56:67e3d9608403 597 // Allocate structure
Jeej 56:67e3d9608403 598 kal_circ_buf_t* circ = (kal_circ_buf_t*)KAL_MALLOC(sizeof(kal_circ_buf_t) + (dim * len - 1) * sizeof(s16));
Jeej 56:67e3d9608403 599
Jeej 56:67e3d9608403 600 // Init structure
Jeej 56:67e3d9608403 601 circ->dim = dim;
Jeej 56:67e3d9608403 602 circ->len = len;
Jeej 56:67e3d9608403 603 circ->curr = 0;
Jeej 56:67e3d9608403 604 return circ;
Jeej 56:67e3d9608403 605 }
Jeej 56:67e3d9608403 606
Jeej 56:67e3d9608403 607 //======================================================================
Jeej 56:67e3d9608403 608 // kal_circ_buf_free
Jeej 56:67e3d9608403 609 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 610 /// @brief Free circular buffer
Jeej 56:67e3d9608403 611 /// @param circ kal_circ_buf_t* Pointer to the circular buffer structure
Jeej 56:67e3d9608403 612 /// @return void
Jeej 56:67e3d9608403 613 //======================================================================
Jeej 56:67e3d9608403 614 _public void kal_circ_buf_free(kal_circ_buf_t* circ)
Jeej 56:67e3d9608403 615 {
Jeej 56:67e3d9608403 616 KAL_FREE(circ);
Jeej 56:67e3d9608403 617 }
Jeej 56:67e3d9608403 618
Jeej 56:67e3d9608403 619 //======================================================================
Jeej 56:67e3d9608403 620 // kal_circ_buf_init
Jeej 56:67e3d9608403 621 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 622 /// @brief Init the buffer with the same element
Jeej 56:67e3d9608403 623 /// @param v s16* d-dimensional vector to write
Jeej 56:67e3d9608403 624 /// @param circ kal_circ_buf_t* Pointer to the circular buffer structure
Jeej 56:67e3d9608403 625 /// @return void
Jeej 56:67e3d9608403 626 //======================================================================
Jeej 56:67e3d9608403 627 _public void kal_circ_buf_init(s16* v, kal_circ_buf_t* circ)
Jeej 56:67e3d9608403 628 {
Jeej 56:67e3d9608403 629 u8 i;
Jeej 56:67e3d9608403 630 s16 *pb = &circ->buf[0];
Jeej 56:67e3d9608403 631 for (i = 0; i < circ->len; ++i)
Jeej 56:67e3d9608403 632 {
Jeej 56:67e3d9608403 633 u8 j;
Jeej 56:67e3d9608403 634 s16* pv = v;
Jeej 56:67e3d9608403 635 for (j = 0; j < circ->dim; ++j)
Jeej 56:67e3d9608403 636 {
Jeej 56:67e3d9608403 637 *pb++ = *pv++;
Jeej 56:67e3d9608403 638 }
Jeej 56:67e3d9608403 639 }
Jeej 56:67e3d9608403 640 }
Jeej 56:67e3d9608403 641
Jeej 56:67e3d9608403 642 //======================================================================
Jeej 56:67e3d9608403 643 // kal_circ_buf_add
Jeej 56:67e3d9608403 644 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 645 /// @brief Add new element of type s16 in buffer, erase oldest element
Jeej 56:67e3d9608403 646 /// @param v s16* d-dimensional vector to write
Jeej 56:67e3d9608403 647 /// @param circ kal_circ_buf_t* Pointer to the circular buffer structure
Jeej 56:67e3d9608403 648 /// @return void
Jeej 56:67e3d9608403 649 //======================================================================
Jeej 56:67e3d9608403 650 _public void kal_circ_buf_add(s16* v, kal_circ_buf_t* circ)
Jeej 56:67e3d9608403 651 {
Jeej 56:67e3d9608403 652 u8 i;
Jeej 56:67e3d9608403 653 s16* p = &circ->buf[0] + (circ->dim * circ->curr);
Jeej 56:67e3d9608403 654 for (i = 0; i < circ->dim; ++i)
Jeej 56:67e3d9608403 655 {
Jeej 56:67e3d9608403 656 *p++ = *v++;
Jeej 56:67e3d9608403 657 }
Jeej 56:67e3d9608403 658 circ->curr++;
Jeej 56:67e3d9608403 659 if (circ->curr == circ->len)
Jeej 56:67e3d9608403 660 {
Jeej 56:67e3d9608403 661 circ->curr = 0;
Jeej 56:67e3d9608403 662 }
Jeej 56:67e3d9608403 663 }
Jeej 56:67e3d9608403 664
Jeej 56:67e3d9608403 665 //======================================================================
Jeej 56:67e3d9608403 666 // kal_lp_filter
Jeej 56:67e3d9608403 667 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 668 /// @brief Low Pass Filter with forget factor
Jeej 56:67e3d9608403 669 /// @param lp s16* Pointer to the LP sample
Jeej 56:67e3d9608403 670 /// @param s s16* Pointer to the new sample
Jeej 56:67e3d9608403 671 /// @param dim u8 Number of dimensions of the sample
Jeej 56:67e3d9608403 672 /// @param ff u8 Forget factor
Jeej 56:67e3d9608403 673 /// @return void
Jeej 56:67e3d9608403 674 //======================================================================
Jeej 56:67e3d9608403 675 _public void kal_lp_filter(s16* lpf, s16* s, u8 dim, u8 ff)
Jeej 56:67e3d9608403 676 {
Jeej 56:67e3d9608403 677 if (ff)
Jeej 56:67e3d9608403 678 {
Jeej 56:67e3d9608403 679 u8 i;
Jeej 56:67e3d9608403 680 for (i = 0; i < dim; ++i)
Jeej 56:67e3d9608403 681 {
Jeej 56:67e3d9608403 682 s16 a = *lpf;
Jeej 56:67e3d9608403 683 s16 b = *s++;
Jeej 56:67e3d9608403 684 a += ((b - a) / ff);
Jeej 56:67e3d9608403 685 *lpf++ = a;
Jeej 56:67e3d9608403 686 }
Jeej 56:67e3d9608403 687 }
Jeej 56:67e3d9608403 688 }
Jeej 56:67e3d9608403 689
Jeej 56:67e3d9608403 690 //======================================================================
Jeej 56:67e3d9608403 691 // kal_xor
Jeej 56:67e3d9608403 692 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 693 /// @brief xor two vectors
Jeej 56:67e3d9608403 694 /// @param buf u8* inout stream
Jeej 56:67e3d9608403 695 /// @param iv u8* in stream to XOR
Jeej 56:67e3d9608403 696 /// @retval void
Jeej 56:67e3d9608403 697 //======================================================================
Jeej 56:67e3d9608403 698 _public void kal_xor(u8* buf, u8* iv, u16 len)
Jeej 56:67e3d9608403 699 {
Jeej 56:67e3d9608403 700 u16 i;
Jeej 56:67e3d9608403 701 for(i = 0; i < len; ++i)
Jeej 56:67e3d9608403 702 {
Jeej 56:67e3d9608403 703 buf[i] ^= iv[i];
Jeej 56:67e3d9608403 704 }
Jeej 56:67e3d9608403 705 }
Jeej 56:67e3d9608403 706
Jeej 56:67e3d9608403 707 //======================================================================
Jeej 56:67e3d9608403 708 // kal_sort
Jeej 56:67e3d9608403 709 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 710 /// @brief Sort u16 buffer from smaller to bigger error
Jeej 56:67e3d9608403 711 /// @param inout u16* inout buffer
Jeej 56:67e3d9608403 712 /// @param ref u16 comparison reference
Jeej 56:67e3d9608403 713 /// @param len u8 buffer length
Jeej 56:67e3d9608403 714 /// @return void
Jeej 56:67e3d9608403 715 //======================================================================
Jeej 56:67e3d9608403 716 _public void kal_sort(u16* inout, u16 ref, u8 len)
Jeej 56:67e3d9608403 717 {
Jeej 56:67e3d9608403 718 u8 i,j;
Jeej 56:67e3d9608403 719 for (i = 0; i < len; i++)
Jeej 56:67e3d9608403 720 {
Jeej 56:67e3d9608403 721 u16 tmp, min = MAX_U16;
Jeej 56:67e3d9608403 722 u8 k = len;
Jeej 56:67e3d9608403 723 for (j = i; j < len; j++)
Jeej 56:67e3d9608403 724 {
Jeej 56:67e3d9608403 725 s32 diff = inout[j] - ref;
Jeej 56:67e3d9608403 726 u16 d = KAL_SAT_U16(kal_abs_s32(diff));
Jeej 56:67e3d9608403 727 if (d <= min)
Jeej 56:67e3d9608403 728 {
Jeej 56:67e3d9608403 729 k = j;
Jeej 56:67e3d9608403 730 min = d;
Jeej 56:67e3d9608403 731 }
Jeej 56:67e3d9608403 732 }
Jeej 56:67e3d9608403 733 tmp = inout[i];
Jeej 56:67e3d9608403 734 inout[i] = inout[k];
Jeej 56:67e3d9608403 735 inout[k] = tmp;
Jeej 56:67e3d9608403 736 }
Jeej 56:67e3d9608403 737 }