5,9 → 5,6 |
// For scope debugging only! |
#include "output.h" |
|
// Debug |
#include "uart0.h" |
|
const int16_t SIN_TABLE[] PROGMEM = { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.01745240643728351 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.03489949670250097 * MATH_UNIT_FACTOR + 0.5), |
97,8 → 94,8 |
(int16_t) (0.9975640502598242 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9986295347545738 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9993908270190958 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9998476951563913 * MATH_UNIT_FACTOR + 0.5), (int16_t) (1.0 |
* MATH_UNIT_FACTOR) }; |
(int16_t) (0.9998476951563913 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (MATH_UNIT_FACTOR) + 0.5 }; |
|
const int16_t TAN_TABLE[] PROGMEM |
= { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5), |
193,43 → 190,50 |
(int16_t) (57.289961630759876 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (32767) }; |
|
int16_t int_sin(int32_t arg) { |
int16_t sin_360(int16_t arg) { |
int8_t sign; |
int16_t result; |
int16_t argp = arg / MATH_DRG_FACTOR; |
argp %= 360; |
if (argp < 0) { |
argp = -argp; |
arg %= 360; |
if (arg < 0) { |
arg = -arg; |
sign = -1; |
} else { |
sign = 1; |
} |
if (argp >= 90) { |
argp = 180 - argp; |
if (arg > 180) { |
arg = 360 - arg; |
sign = -sign; |
} |
result = pgm_read_word(&SIN_TABLE[(uint8_t) argp]); |
if (arg > 90) { |
arg = 180 - arg; |
} |
result = pgm_read_word(&SIN_TABLE[(uint8_t) arg]); |
return (sign == 1) ? result : -result; |
} |
|
int16_t int_cos(int32_t arg) { |
if (arg > 90L * MATH_DRG_FACTOR) |
return int_sin(arg + (90L - 360L) * MATH_DRG_FACTOR); |
return int_sin(arg + 90L * MATH_DRG_FACTOR); |
int16_t cos_360(int16_t arg) { |
return sin_360(arg + 90); |
} |
|
int16_t int_tan(int32_t arg) { |
int16_t tan_360(int16_t arg) { |
int8_t sign = 1; |
int16_t result; |
int16_t argp = arg / MATH_DRG_FACTOR; |
if (argp >= 90) { |
argp = 180 - argp; |
sign = -1; |
} else if (argp < -90) { |
argp += 180; |
} else if (argp < 0) { |
argp = -argp; |
sign = -1; |
if (arg < 0) { |
arg = -arg; |
sign = -1; |
} |
result = pgm_read_word(&TAN_TABLE[(uint8_t) argp]); |
if (arg >= 90) { |
arg = 180 - arg; |
sign = -sign; |
} |
result = pgm_read_word(&TAN_TABLE[(uint8_t) arg]); |
return (sign == 1) ? result : -result; |
} |
|
void intervalWrap(int32_t *number, int32_t limit) { |
if (*number >= limit) { |
*number -= limit; // 360 deg. wrap |
} else if (*number < 0) { |
*number += limit; |
} |
} |