Subversion Repositories FlightCtrl

Rev

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

Rev 2044 Rev 2045
Line 95... Line 95...
95 
    (int16_t) (0.9945218953682733 * MATH_UNIT_FACTOR + 0.5),
 95 
    (int16_t) (0.9945218953682733 * MATH_UNIT_FACTOR + 0.5),
96 
    (int16_t) (0.9961946980917455 * MATH_UNIT_FACTOR + 0.5),
 96 
    (int16_t) (0.9961946980917455 * MATH_UNIT_FACTOR + 0.5),
97 
    (int16_t) (0.9975640502598242 * MATH_UNIT_FACTOR + 0.5),
 97 
    (int16_t) (0.9975640502598242 * MATH_UNIT_FACTOR + 0.5),
98 
    (int16_t) (0.9986295347545738 * MATH_UNIT_FACTOR + 0.5),
 98 
    (int16_t) (0.9986295347545738 * MATH_UNIT_FACTOR + 0.5),
99 
    (int16_t) (0.9993908270190958 * MATH_UNIT_FACTOR + 0.5),
 99 
    (int16_t) (0.9993908270190958 * MATH_UNIT_FACTOR + 0.5),
100 
    (int16_t) (0.9998476951563913 * MATH_UNIT_FACTOR + 0.5), (int16_t) (1.0
 100 
    (int16_t) (0.9998476951563913 * MATH_UNIT_FACTOR + 0.5),
101 
        * MATH_UNIT_FACTOR) };
 101 
    (int16_t) (MATH_UNIT_FACTOR) + 0.5 };
Line 102... Line 102...
102 
 
 102 
 
103 
const int16_t TAN_TABLE[] PROGMEM
 103 
const int16_t TAN_TABLE[] PROGMEM
104 
    = { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5),
 104 
    = { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5),
105 
        (int16_t) (0.017455064928217585 * MATH_UNIT_FACTOR + 0.5),
 105 
        (int16_t) (0.017455064928217585 * MATH_UNIT_FACTOR + 0.5),
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 sin_mkdegrees(int32_t arg) {
 196 
int16_t sin_360(int16_t arg) {
197 
  int8_t sign;
 197 
  int8_t sign;
198 
  int16_t result;
 - 
 
199 
  int16_t argp = arg / MATH_DRG_FACTOR;
 198 
  int16_t result;
200 
  argp %= 360;
 199 
  arg %= 360;
201 
  if (argp < 0) {
 200 
  if (arg < 0) {
202 
    argp = -argp;
 201 
    arg = -arg;
203 
    sign = -1;
 202 
    sign = -1;
204 
  } else {
 203 
  } else {
205 
    sign = 1;
 204 
    sign = 1;
206 
  }
 205 
  }
207 
  if (argp >= 90) {
 206 
  if (arg > 180) {
- 
 
 207 
    arg = 360 - arg;
208 
    argp = 180 - argp;
 208 
    sign = -sign;
- 
 
 209 
  }
- 
 
 210 
  if (arg > 90) {
- 
 
 211 
    arg = 180 - arg;
209 
  }
 212 
  }
210 
  result = pgm_read_word(&SIN_TABLE[(uint8_t) argp]);
 213 
  result = pgm_read_word(&SIN_TABLE[(uint8_t) arg]);
211 
  return (sign == 1) ? result : -result;
 214 
  return (sign == 1) ? result : -result;
Line 212... Line 215...
212 
}
 215 
}
213 
 
 - 
 
214 
int16_t cos_mkdegrees(int32_t arg) {
 - 
 
215 
  if (arg > 90L * MATH_DRG_FACTOR)
 216 
 
216 
    return sin_mkdegrees(arg + (90L - 360L) * MATH_DRG_FACTOR);
 217 
int16_t cos_360(int16_t arg) {
Line 217... Line 218...
217 
  return sin_mkdegrees(arg + 90L * MATH_DRG_FACTOR);
 218 
  return sin_360(arg + 90);
218 
}
 219 
}
219 
 
 220 
 
220 
int16_t tan_mkdegrees(int32_t arg) {
 - 
 
221 
  int8_t sign = 1;
 - 
 
222 
  int16_t result;
 - 
 
223 
  int16_t argp = arg / MATH_DRG_FACTOR;
 - 
 
224 
  if (argp >= 90) {
 - 
 
225 
    argp = 180 - argp;
 - 
 
226 
    sign = -1;
 - 
 
227 
  } else if (argp < -90) {
 - 
 
228 
    argp += 180;
 - 
 
229 
  } else if (argp < 0) {
 - 
 
230 
    argp = -argp;
 - 
 
231 
    sign = -1;
 - 
 
232 
  }
 - 
 
233 
  result = pgm_read_word(&TAN_TABLE[(uint8_t) argp]);
 - 
 
234 
  return (sign == 1) ? result : -result;
 - 
 
235 
}
 - 
 
236 
 
 - 
 
237 
int16_t sin_360(int16_t arg) {
 - 
 
238 
  int8_t sign;
 221 
int16_t tan_360(int16_t arg) {
239 
  int16_t result;
 222 
  int8_t sign = 1;
240 
  arg %= 360;
 223 
  int16_t result;
241 
  if (arg < 0) {
 - 
 
242 
    arg = -arg;
 - 
 
243 
    sign = -1;
 224 
  if (arg < 0) {
244 
  } else {
 225 
      arg = -arg;
245 
    sign = 1;
 226 
      sign = -1;
- 
 
 227 
  }
246 
  }
 228 
  if (arg >= 90) {
247 
  if (arg >= 90) {
 229 
    arg = 180 - arg;
248 
    arg = 180 - arg;
 230 
    sign = -sign;
249 
  }
 231 
  }
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)
 -