1 /*******************************************************************
2 Components for embedded applications builded for
3 laboratory and medical instruments firmware
5 pxmc_sin_fixed.h - generic multi axis motion controller
6 fixed sine approximation
8 (C) 2001-2015 by Pavel Pisa pisa@cmp.felk.cvut.cz
9 (C) 2002-2015 by PiKRON Ltd. http://www.pikron.com
11 This file can be used and copied according to next
13 - GPL - GNU Public License
14 - other license provided by project originators
16 *******************************************************************/
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
31 #define PXMC_SIN_FIX_PI2 0x40000000
32 #define PXMC_SIN_FIX_2PI3 0x55555555
34 extern const uint32_t pxmc_sin_fixed_table[];
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)
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
45 int32_t pxmc_sin_fixed_inline(uint32_t x, int res_unit_bits)
56 ti = x >> PXMC_SIN_FIX_IDX_SLR;
57 xd = (x & PXMC_SIN_FIX_XD_MASK) >> PXMC_SIN_FIX_XD_SLR;
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;
64 yl += ((int32_t)b * xd) >> PXMC_SIN_FIX_B_XD_SAR;
66 yl += yl & (1 << (29 - res_unit_bits));
67 yl >>= 30 - res_unit_bits;
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)
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
83 void pxmc_sincos_fixed_inline(int32_t *pysin, int32_t *pycos,
84 uint32_t x, int res_unit_bits)
96 ti = x >> PXMC_SIN_FIX_IDX_SLR;
97 xd = (x & PXMC_SIN_FIX_XD_MASK) >> PXMC_SIN_FIX_XD_SLR;
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;
104 ysin += ((int32_t)b * xd) >> PXMC_SIN_FIX_B_XD_SAR;
106 ysin += ysin & (1 << (29 - res_unit_bits));
107 ysin >>= 30 - res_unit_bits;
109 ti = (uint32_t)(x + PXMC_SIN_FIX_PI2) >> PXMC_SIN_FIX_IDX_SLR;
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;
116 ycos += ((int32_t)b * xd) >> PXMC_SIN_FIX_B_XD_SAR;
118 ycos += ycos & (1 << (29 - res_unit_bits));
119 ycos >>= 30 - res_unit_bits;
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
129 * Returns value one if input is in the 0 or PI +/- 10 deg range.
132 int pxmc_sin_fixed_zic_inline(uint32_t x)
136 ti = x >> PXMC_SIN_FIX_IDX_SLR;
138 return (pxmc_sin_fixed_table[ti] >> PXMC_SIN_FIX_ZIC_BIT) & 1;