Subversion Repositories FlightCtrl

Rev

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

Rev 1910 Rev 2099
Line 3... Line 3...
3 
#include <avr/pgmspace.h>
 3 
#include <avr/pgmspace.h>
Line 4... Line 4...
4 
 
 4 
 
5 
// For scope debugging only!
 5 
// For scope debugging only!
Line 6... Line -...
6 
#include "output.h"
 - 
 
7 
 
 - 
 
8 
// Debug
 - 
 
9 
#include "uart0.h"
 6 
#include "output.h"
10 
 
 7 
 
11 
const int16_t SIN_TABLE[] PROGMEM = { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5),
 8 
const int16_t SIN_TABLE[] PROGMEM = { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5),
12 
    (int16_t) (0.01745240643728351 * MATH_UNIT_FACTOR + 0.5),
 9 
    (int16_t) (0.01745240643728351 * MATH_UNIT_FACTOR + 0.5),
13 
    (int16_t) (0.03489949670250097 * MATH_UNIT_FACTOR + 0.5),
 10 
    (int16_t) (0.03489949670250097 * MATH_UNIT_FACTOR + 0.5),
Line 95... Line 92...
95 
    (int16_t) (0.9945218953682733 * MATH_UNIT_FACTOR + 0.5),
 92 
    (int16_t) (0.9945218953682733 * MATH_UNIT_FACTOR + 0.5),
96 
    (int16_t) (0.9961946980917455 * MATH_UNIT_FACTOR + 0.5),
 93 
    (int16_t) (0.9961946980917455 * MATH_UNIT_FACTOR + 0.5),
97 
    (int16_t) (0.9975640502598242 * MATH_UNIT_FACTOR + 0.5),
 94 
    (int16_t) (0.9975640502598242 * MATH_UNIT_FACTOR + 0.5),
98 
    (int16_t) (0.9986295347545738 * MATH_UNIT_FACTOR + 0.5),
 95 
    (int16_t) (0.9986295347545738 * MATH_UNIT_FACTOR + 0.5),
99 
    (int16_t) (0.9993908270190958 * MATH_UNIT_FACTOR + 0.5),
 96 
    (int16_t) (0.9993908270190958 * MATH_UNIT_FACTOR + 0.5),
100 
    (int16_t) (0.9998476951563913 * MATH_UNIT_FACTOR + 0.5), (int16_t) (1.0
 97 
    (int16_t) (0.9998476951563913 * MATH_UNIT_FACTOR + 0.5),
101 
        * MATH_UNIT_FACTOR) };
 98 
    (int16_t) (MATH_UNIT_FACTOR) + 0.5 };
Line 102... Line 99...
102 
 
 99 
 
103 
const int16_t TAN_TABLE[] PROGMEM
 100 
const int16_t TAN_TABLE[] PROGMEM
104 
    = { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5),
 101 
    = { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5),
105 
        (int16_t) (0.017455064928217585 * MATH_UNIT_FACTOR + 0.5),
 102 
        (int16_t) (0.017455064928217585 * MATH_UNIT_FACTOR + 0.5),
Line 191... Line 188...
191 
        (int16_t) (19.08113668772816 * MATH_UNIT_FACTOR + 0.5),
 188 
        (int16_t) (19.08113668772816 * MATH_UNIT_FACTOR + 0.5),
192 
        (int16_t) (28.636253282915515 * MATH_UNIT_FACTOR + 0.5),
 189 
        (int16_t) (28.636253282915515 * MATH_UNIT_FACTOR + 0.5),
193 
        (int16_t) (57.289961630759876 * MATH_UNIT_FACTOR + 0.5),
 190 
        (int16_t) (57.289961630759876 * MATH_UNIT_FACTOR + 0.5),
194 
        (int16_t) (32767) };
 191 
        (int16_t) (32767) };
Line 195... Line 192...
195 
 
 192 
 
196 
int16_t int_sin(int32_t arg) {
 193 
int16_t sin_360(int16_t arg) {
197 
  int8_t sign;
 194 
  int8_t sign;
198 
  int16_t result;
 - 
 
199 
  int16_t argp = arg / MATH_DRG_FACTOR;
 195 
  int16_t result;
200 
  argp %= 360;
 196 
  arg %= 360;
201 
  if (argp < 0) {
 197 
  if (arg < 0) {
202 
    argp = -argp;
 198 
    arg = -arg;
203 
    sign = -1;
 199 
    sign = -1;
204 
  } else {
 200 
  } else {
205 
    sign = 1;
 201 
    sign = 1;
- 
 
 202 
  }
- 
 
 203 
  if (arg > 180) {
- 
 
 204 
    arg = 360 - arg;
- 
 
 205 
    sign = -sign;
206 
  }
 206 
  }
207 
  if (argp >= 90) {
 207 
  if (arg > 90) {
208 
    argp = 180 - argp;
 208 
    arg = 180 - arg;
209 
  }
 209 
  }
210 
  result = pgm_read_word(&SIN_TABLE[(uint8_t) argp]);
 210 
  result = pgm_read_word(&SIN_TABLE[(uint8_t) arg]);
211 
  return (sign == 1) ? result : -result;
 211 
  return (sign == 1) ? result : -result;
Line 212... Line 212...
212 
}
 212 
}
213 
 
 - 
 
214 
int16_t int_cos(int32_t arg) {
 - 
 
215 
  if (arg > 90L * MATH_DRG_FACTOR)
 213 
 
216 
    return int_sin(arg + (90L - 360L) * MATH_DRG_FACTOR);
 214 
int16_t cos_360(int16_t arg) {
Line 217... Line 215...
217 
  return int_sin(arg + 90L * MATH_DRG_FACTOR);
 215 
  return sin_360(arg + 90);
218 
}
 216 
}
219 
 
 217 
 
220 
int16_t int_tan(int32_t arg) {
 - 
 
221 
  int8_t sign = 1;
 218 
int16_t tan_360(int16_t arg) {
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) {
 219 
  int8_t sign = 1;
228 
    argp += 180;
 220 
  int16_t result;
229 
  } else if (argp < 0) {
 221 
  if (arg < 0) {
- 
 
 222 
      arg = -arg;
- 
 
 223 
      sign = -1;
- 
 
 224 
  }
- 
 
 225 
  if (arg >= 90) {
230 
    argp = -argp;
 226 
    arg = 180 - arg;
231 
    sign = -1;
 227 
    sign = -sign;
232 
  }
 228 
  }
- 
 
 229 
  result = pgm_read_word(&TAN_TABLE[(uint8_t) arg]);
- 
 
 230 
  return (sign == 1) ? result : -result;
- 
 
 231 
}
- 
 
 232 
 
- 
 
 233 
void intervalWrap(int32_t *number, int32_t limit) {
- 
 
 234 
  if (*number >= limit) {
- 
 
 235 
    *number -= limit; // 360 deg. wrap
- 
 
 236 
  } else if (*number < 0) {