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
 
-