Subversion Repositories FlightCtrl

Rev

Rev 1872 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1872 Rev 2044
Line 191... Line 191...
191 
        (int16_t) (19.08113668772816 * MATH_UNIT_FACTOR + 0.5),
 191 
        (int16_t) (19.08113668772816 * MATH_UNIT_FACTOR + 0.5),
192 
        (int16_t) (28.636253282915515 * MATH_UNIT_FACTOR + 0.5),
 192 
        (int16_t) (28.636253282915515 * MATH_UNIT_FACTOR + 0.5),
193 
        (int16_t) (57.289961630759876 * MATH_UNIT_FACTOR + 0.5),
 193 
        (int16_t) (57.289961630759876 * MATH_UNIT_FACTOR + 0.5),
194 
        (int16_t) (32767) };
 194 
        (int16_t) (32767) };
Line 195... Line 195...
195 
 
 195 
 
196 
int16_t int_sin(int32_t arg) {
 196 
int16_t sin_mkdegrees(int32_t arg) {
197 
  int8_t sign;
 197 
  int8_t sign;
198 
  int16_t result;
 198 
  int16_t result;
199 
  int16_t argp = arg / MATH_DRG_FACTOR;
 199 
  int16_t argp = arg / MATH_DRG_FACTOR;
200 
  argp %= 360;
 200 
  argp %= 360;
Line 209... Line 209...
209 
  }
 209 
  }
210 
  result = pgm_read_word(&SIN_TABLE[(uint8_t) argp]);
 210 
  result = pgm_read_word(&SIN_TABLE[(uint8_t) argp]);
211 
  return (sign == 1) ? result : -result;
 211 
  return (sign == 1) ? result : -result;
212 
}
 212 
}
Line 213... Line 213...
213 
 
 213 
 
214 
int16_t int_cos(int32_t arg) {
 214 
int16_t cos_mkdegrees(int32_t arg) {
215 
  if (arg > 90L * MATH_DRG_FACTOR)
 215 
  if (arg > 90L * MATH_DRG_FACTOR)
216 
    return int_sin(arg + (90L - 360L) * MATH_DRG_FACTOR);
 216 
    return sin_mkdegrees(arg + (90L - 360L) * MATH_DRG_FACTOR);
217 
  return int_sin(arg + 90L * MATH_DRG_FACTOR);
 217 
  return sin_mkdegrees(arg + 90L * MATH_DRG_FACTOR);
Line 218... Line 218...
218 
}
 218 
}
219 
 
 219 
 
220 
int16_t int_tan(int32_t arg) {
 220 
int16_t tan_mkdegrees(int32_t arg) {
221 
  int8_t sign = 1;
 221 
  int8_t sign = 1;
222 
  int16_t result;
 222 
  int16_t result;
223 
  int16_t argp = arg / MATH_DRG_FACTOR;
 223 
  int16_t argp = arg / MATH_DRG_FACTOR;
Line 231... Line 231...
231 
    sign = -1;
 231 
    sign = -1;
232 
  }
 232 
  }
233 
  result = pgm_read_word(&TAN_TABLE[(uint8_t) argp]);
 233 
  result = pgm_read_word(&TAN_TABLE[(uint8_t) argp]);
234 
  return (sign == 1) ? result : -result;
 234 
  return (sign == 1) ? result : -result;
235 
}
 235 
}
- 
 
 236 
 
- 
 
 237 
int16_t sin_360(int16_t arg) {
- 
 
 238 
  int8_t sign;
- 
 
 239 
  int16_t result;
- 
 
 240 
  arg %= 360;
- 
 
 241 
  if (arg < 0) {
- 
 
 242 
    arg = -arg;
- 
 
 243 
    sign = -1;
- 
 
 244 
  } else {
- 
 
 245 
    sign = 1;
- 
 
 246 
  }
- 
 
 247 
  if (arg >= 90) {
- 
 
 248 
    arg = 180 - arg;
- 
 
 249 
  }
- 
 
 250 
  result = pgm_read_word(&SIN_TABLE[(uint8_t) arg]);
- 
 
 251 
  return (sign == 1) ? result : -result;
- 
 
 252 
}
- 
 
 253 
 
- 
 
 254 
int16_t cos_360(int16_t arg) {
- 
 
 255 
  if (arg > 90L)
- 
 
 256 
    return sin_360(arg + 90- 360);
- 
 
 257 
  return sin_360(arg + 90);
- 
 
 258 
}