Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1910 → Rev 2099

/branches/dongfang_FC_fixedwing/dongfangMath.c
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;
}
}