Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2043 → Rev 2044

/branches/dongfang_FC_rewrite/dongfangMath.c
193,7 → 193,7
(int16_t) (57.289961630759876 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (32767) };
 
int16_t int_sin(int32_t arg) {
int16_t sin_mkdegrees(int32_t arg) {
int8_t sign;
int16_t result;
int16_t argp = arg / MATH_DRG_FACTOR;
211,13 → 211,13
return (sign == 1) ? result : -result;
}
 
int16_t int_cos(int32_t arg) {
int16_t cos_mkdegrees(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);
return sin_mkdegrees(arg + (90L - 360L) * MATH_DRG_FACTOR);
return sin_mkdegrees(arg + 90L * MATH_DRG_FACTOR);
}
 
int16_t int_tan(int32_t arg) {
int16_t tan_mkdegrees(int32_t arg) {
int8_t sign = 1;
int16_t result;
int16_t argp = arg / MATH_DRG_FACTOR;
233,3 → 233,26
result = pgm_read_word(&TAN_TABLE[(uint8_t) argp]);
return (sign == 1) ? result : -result;
}
 
int16_t sin_360(int16_t arg) {
int8_t sign;
int16_t result;
arg %= 360;
if (arg < 0) {
arg = -arg;
sign = -1;
} else {
sign = 1;
}
if (arg >= 90) {
arg = 180 - arg;
}
result = pgm_read_word(&SIN_TABLE[(uint8_t) arg]);
return (sign == 1) ? result : -result;
}
 
int16_t cos_360(int16_t arg) {
if (arg > 90L)
return sin_360(arg + 90- 360);
return sin_360(arg + 90);
}