Subversion Repositories FlightCtrl

Rev

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

Rev 1877 Rev 1887
Line 6... Line 6...
6 
 
 6 
 
7 
// For scope debugging only!
 7 
// For scope debugging only!
Line 8... Line 8...
8 
#include "output.h"
 8 
#include "output.h"
9 
 
 9 
 
10 
// = cos^2(45 degs).
 10 
// = cos^2(45 degs).
Line 11... Line 11...
11 
const int32_t FACTORSQUARED = (int32_t) (1<<(MATH_UNIT_FACTOR_LOG*2));
 11 
// const int32_t FACTORSQUARED = 1L << (MATH_UNIT_FACTOR_LOG * 2);
12 
const int32_t MINPROJECTION = (int32_t) (1<<(MATH_UNIT_FACTOR_LOG*2-1));
 12 
const int32_t MINPROJECTION = 1L << (MATH_UNIT_FACTOR_LOG * 2 - 9);
13 
 
 13 
 
14 
// Takes 380 - 400 usec. Way too slow.
 14 
// Takes 380 - 400 usec. Way too slow.
15 
// With static MINPROJECTION: 220 usec.
 15 
// With static MINPROJECTION: 220 usec.
16 
uint16_t AC_getThrottle(uint16_t throttle) {
 16 
uint16_t AC_getThrottle(uint16_t throttle) {
17 
  int32_t projection;
 17 
  int32_t projection;
18 
  uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3
 18 
  uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3
- 
 
 19 
  int16_t deltaThrottle, y;
19 
  int16_t deltaThrottle;
 20 
 
- 
 
 21 
  projection = (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]);
- 
 
 22 
  projection >>= 8;
- 
 
 23 
 
20 
 
 24 
  if (projection < 0) {
21 
  projection =  (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]);
 25 
    // Case not yet considered!
22 
 
 26 
    y = 0;
23 
  /*
 27 
  } else {
24 
    if (projection < MINPROJECTION && projection >= 0) {
 28 
    if (projection < MINPROJECTION && projection >= 0) {
25 
    deltaThrottle = 0;
 - 
 
26 
    } else if (projection > -MINPROJECTION && projection < 0) {
 29 
      projection = MINPROJECTION;
27 
    // projection = -MINPROJECITON;
 30 
    } else if (projection > -MINPROJECTION && projection < 0) {
- 
 
 31 
      projection = -MINPROJECTION;
- 
 
 32 
    } else {
28 
    deltaThrottle = 0;
 33 
    }
29 
    } else {
 34 
    y = ((int32_t) throttle << (MATH_UNIT_FACTOR_LOG * 2 - 8)) / projection
30 
  */
 35 
        - throttle;
31 
 
 - 
 
32 
  deltaThrottle = (((int32_t) effect * throttle) << (MATH_UNIT_FACTOR*2)) / (projection * 100) - (1 << (MATH_UNIT_FACTOR*2));
 36 
  }
33 
  // DebugOut.Analog[13] = deltaThrottle;
 37 
  deltaThrottle = ((int32_t)y * effect) >> 6;
34 
 
 -