pmsm-control/test_sw/pxmc_sin_fixed.h

   1 /*******************************************************************
   2   Components for embedded applications builded for
   3   laboratory and medical instruments firmware
   4
   5   pxmc_sin_fixed.h - generic multi axis motion controller
   6              fixed sine approximation
   7
   8   (C) 2001-2015 by Pavel Pisa pisa@cmp.felk.cvut.cz
   9   (C) 2002-2015 by PiKRON Ltd. http://www.pikron.com
  10
  11   This file can be used and copied according to next
  12   license alternatives
  13    - GPL - GNU Public License
  14    - other license provided by project originators
  15
  16  *******************************************************************/
  17
  18 #include <stdint.h>
  19
  20 #define PXMC_SIN_FIX_TAB_BITS 9
  21 #define PXMC_SIN_FIX_IDX_SLR  23
  22 #define PXMC_SIN_FIX_XD_MASK  0x007fffff
  23 #define PXMC_SIN_FIX_XD_SLR   8
  24 #define PXMC_SIN_FIX_A_MASK   0xffffc000
  25 #define PXMC_SIN_FIX_B_SLL    19
  26 #define PXMC_SIN_FIX_B_SAR    16
  27 #define PXMC_SIN_FIX_B_XD_SAR 6
  28 #define PXMC_SIN_FIX_ZIC_MASK 0x00002000
  29 #define PXMC_SIN_FIX_ZIC_BIT  13
  30
  31 #define PXMC_SIN_FIX_PI2      0x40000000
  32 #define PXMC_SIN_FIX_2PI3     0x55555555
  33
  34 extern const uint32_t pxmc_sin_fixed_table[];
  35
  36 /**
  37  * pxmc_sin_fixed_inline - fixed math sine computation
  38  * @x:  The argument value - 2^32 corresponds to 2 * PI or 360 deg
  39  * @res_unit_bits: number of fraction bits of the result (default 16)
  40  *
  41  * Returns sine of value @x argument where unit is 2^16, i.e.
  42  * return value 0x10000 is equivalent to +1.0 and -0x10000 to -1.0
  43  */
  44 static inline
  45 int32_t pxmc_sin_fixed_inline(uint32_t x, int res_unit_bits)
  46 {
  47   uint32_t tabval;
  48   unsigned int ti;
  49   int32_t a;
  50   int b, xd;
  51   int32_t yl;
  52
  53   if (!res_unit_bits)
  54     res_unit_bits = 16;
  55
  56   ti = x >> PXMC_SIN_FIX_IDX_SLR;
  57   xd = (x & PXMC_SIN_FIX_XD_MASK) >> PXMC_SIN_FIX_XD_SLR;
  58
  59   tabval = pxmc_sin_fixed_table[ti];
  60   a = tabval & PXMC_SIN_FIX_A_MASK;
  61   b = (int32_t)(tabval << PXMC_SIN_FIX_B_SLL) >> PXMC_SIN_FIX_B_SAR;
  62
  63   yl = a;
  64   yl += ((int32_t)b * xd) >> PXMC_SIN_FIX_B_XD_SAR;
  65
  66   yl += yl & (1 << (29 - res_unit_bits));
  67   yl >>= 30 - res_unit_bits;
  68
  69   return yl;
  70 }
  71
  72 /**
  73  * pxmc_sincos_fixed_inline - fixed math sine computation
  74  * @pysin: pointer to location where sine value is returned
  75  * @pycos: pointer to location where cosine value is returned
  76  * @x:  The argument value - 2^32 corresponds to 2 * PI or 360 deg
  77  * @res_unit_bits: number of fraction bits of the result (default 16)
  78  *
  79  * Returns sine and cosine of value @x argument where unit is 2^16, i.e.
  80  * return value 0x10000 is equivalent to +1.0 and -0x10000 to -1.0
  81  */
  82 static inline
  83 void pxmc_sincos_fixed_inline(int32_t *pysin, int32_t *pycos,
  84                               uint32_t x, int res_unit_bits)
  85 {
  86   uint32_t tabval;
  87   unsigned int ti;
  88   int32_t a;
  89   int b, xd;
  90   int32_t ysin;
  91   int32_t ycos;
  92
  93   if (!res_unit_bits)
  94     res_unit_bits = 16;
  95
  96   ti = x >> PXMC_SIN_FIX_IDX_SLR;
  97   xd = (x & PXMC_SIN_FIX_XD_MASK) >> PXMC_SIN_FIX_XD_SLR;
  98
  99   tabval = pxmc_sin_fixed_table[ti];
 100   a = tabval & PXMC_SIN_FIX_A_MASK;
 101   b = (int32_t)(tabval << PXMC_SIN_FIX_B_SLL) >> PXMC_SIN_FIX_B_SAR;
 102
 103   ysin = a;
 104   ysin += ((int32_t)b * xd) >> PXMC_SIN_FIX_B_XD_SAR;
 105
 106   ysin += ysin & (1 << (29 - res_unit_bits));
 107   ysin >>= 30 - res_unit_bits;
 108
 109   ti = (uint32_t)(x + PXMC_SIN_FIX_PI2) >> PXMC_SIN_FIX_IDX_SLR;
 110
 111   tabval = pxmc_sin_fixed_table[ti];
 112   a = tabval & PXMC_SIN_FIX_A_MASK;
 113   b = (int32_t)(tabval << PXMC_SIN_FIX_B_SLL) >> PXMC_SIN_FIX_B_SAR;
 114
 115   ycos = a;
 116   ycos += ((int32_t)b * xd) >> PXMC_SIN_FIX_B_XD_SAR;
 117
 118   ycos += ycos & (1 << (29 - res_unit_bits));
 119   ycos >>= 30 - res_unit_bits;
 120
 121   *pysin = ysin;
 122   *pycos = ycos;
 123 }
 124
 125 /**
 126  * pxmc_sin_fixed_zic_inline - indicate range for +/-10 degree zero current correction
 127  * @x:  The argument value - 2^32 corresponds to 2 * PI or 360 deg
 128  *
 129  * Returns value one if input is in the 0 or PI +/- 10 deg range.
 130  */
 131 static inline
 132 int pxmc_sin_fixed_zic_inline(uint32_t x)
 133 {
 134   unsigned int ti;
 135
 136   ti = x >> PXMC_SIN_FIX_IDX_SLR;
 137
 138   return (pxmc_sin_fixed_table[ti] >> PXMC_SIN_FIX_ZIC_BIT) & 1;
 139 }