+/*******************************************************************
+ Components for embedded applications builded for
+ laboratory and medical instruments firmware
+
+ pxmc_sin_fixed.h - generic multi axis motion controller
+ fixed sine approximation
+
+ (C) 2001-2015 by Pavel Pisa pisa@cmp.felk.cvut.cz
+ (C) 2002-2015 by PiKRON Ltd. http://www.pikron.com
+
+ This file can be used and copied according to next
+ license alternatives
+ - GPL - GNU Public License
+ - other license provided by project originators
+
+ *******************************************************************/
+
+#include <stdint.h>
+
+#define PXMC_SIN_FIX_TAB_BITS 9
+#define PXMC_SIN_FIX_IDX_SLR 23
+#define PXMC_SIN_FIX_XD_MASK 0x007fffff
+#define PXMC_SIN_FIX_XD_SLR 8
+#define PXMC_SIN_FIX_A_MASK 0xffffc000
+#define PXMC_SIN_FIX_B_SLL 19
+#define PXMC_SIN_FIX_B_SAR 16
+#define PXMC_SIN_FIX_B_XD_SAR 6
+#define PXMC_SIN_FIX_ZIC_MASK 0x00002000
+#define PXMC_SIN_FIX_ZIC_BIT 13
+
+#define PXMC_SIN_FIX_PI2 0x40000000
+#define PXMC_SIN_FIX_2PI3 0x55555555
+
+extern const uint32_t pxmc_sin_fixed_table[];
+
+/**
+ * pxmc_sin_fixed_inline - fixed math sine computation
+ * @x: The argument value - 2^32 corresponds to 2 * PI or 360 deg
+ * @res_unit_bits: number of fraction bits of the result (default 16)
+ *
+ * Returns sine of value @x argument where unit is 2^16, i.e.
+ * return value 0x10000 is equivalent to +1.0 and -0x10000 to -1.0
+ */
+static inline
+int32_t pxmc_sin_fixed_inline(uint32_t x, int res_unit_bits)
+{
+ uint32_t tabval;
+ unsigned int ti;
+ int32_t a;
+ int b, xd;
+ int32_t yl;
+
+ if (!res_unit_bits)
+ res_unit_bits = 16;
+
+ ti = x >> PXMC_SIN_FIX_IDX_SLR;
+ xd = (x & PXMC_SIN_FIX_XD_MASK) >> PXMC_SIN_FIX_XD_SLR;
+
+ tabval = pxmc_sin_fixed_table[ti];
+ a = tabval & PXMC_SIN_FIX_A_MASK;
+ b = (int32_t)(tabval << PXMC_SIN_FIX_B_SLL) >> PXMC_SIN_FIX_B_SAR;
+
+ yl = a;
+ yl += ((int32_t)b * xd) >> PXMC_SIN_FIX_B_XD_SAR;
+
+ yl += yl & (1 << (29 - res_unit_bits));
+ yl >>= 30 - res_unit_bits;
+
+ return yl;
+}
+
+/**
+ * pxmc_sincos_fixed_inline - fixed math sine computation
+ * @pysin: pointer to location where sine value is returned
+ * @pycos: pointer to location where cosine value is returned
+ * @x: The argument value - 2^32 corresponds to 2 * PI or 360 deg
+ * @res_unit_bits: number of fraction bits of the result (default 16)
+ *
+ * Returns sine and cosine of value @x argument where unit is 2^16, i.e.
+ * return value 0x10000 is equivalent to +1.0 and -0x10000 to -1.0
+ */
+static inline
+void pxmc_sincos_fixed_inline(int32_t *pysin, int32_t *pycos,
+ uint32_t x, int res_unit_bits)
+{
+ uint32_t tabval;
+ unsigned int ti;
+ int32_t a;
+ int b, xd;
+ int32_t ysin;
+ int32_t ycos;
+
+ if (!res_unit_bits)
+ res_unit_bits = 16;
+
+ ti = x >> PXMC_SIN_FIX_IDX_SLR;
+ xd = (x & PXMC_SIN_FIX_XD_MASK) >> PXMC_SIN_FIX_XD_SLR;
+
+ tabval = pxmc_sin_fixed_table[ti];
+ a = tabval & PXMC_SIN_FIX_A_MASK;
+ b = (int32_t)(tabval << PXMC_SIN_FIX_B_SLL) >> PXMC_SIN_FIX_B_SAR;
+
+ ysin = a;
+ ysin += ((int32_t)b * xd) >> PXMC_SIN_FIX_B_XD_SAR;
+
+ ysin += ysin & (1 << (29 - res_unit_bits));
+ ysin >>= 30 - res_unit_bits;
+
+ ti = (uint32_t)(x + PXMC_SIN_FIX_PI2) >> PXMC_SIN_FIX_IDX_SLR;
+
+ tabval = pxmc_sin_fixed_table[ti];
+ a = tabval & PXMC_SIN_FIX_A_MASK;
+ b = (int32_t)(tabval << PXMC_SIN_FIX_B_SLL) >> PXMC_SIN_FIX_B_SAR;
+
+ ycos = a;
+ ycos += ((int32_t)b * xd) >> PXMC_SIN_FIX_B_XD_SAR;
+
+ ycos += ycos & (1 << (29 - res_unit_bits));
+ ycos >>= 30 - res_unit_bits;
+
+ *pysin = ysin;
+ *pycos = ycos;
+}
+
+/**
+ * pxmc_sin_fixed_zic_inline - indicate range for +/-10 degree zero current correction
+ * @x: The argument value - 2^32 corresponds to 2 * PI or 360 deg
+ *
+ * Returns value one if input is in the 0 or PI +/- 10 deg range.
+ */
+static inline
+int pxmc_sin_fixed_zic_inline(uint32_t x)
+{
+ unsigned int ti;
+
+ ti = x >> PXMC_SIN_FIX_IDX_SLR;
+
+ return (pxmc_sin_fixed_table[ti] >> PXMC_SIN_FIX_ZIC_BIT) & 1;
+}