Subversion Repositories FlightCtrl

Rev

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

Rev 1775 Rev 1821
Line 6... Line 6...
6
 
6
 
7
// For scope debugging only!
7
// For scope debugging only!
Line 8... Line 8...
8
#include "rc.h"
8
#include "rc.h"
9
 
9
 
Line 10... Line 10...
10
// = cos^2(45 degs).
10
// = cos^2(45 degs).
11
const int32_t MINPROJECTION =  (int32_t)MATH_UNIT_FACTOR * MATH_UNIT_FACTOR / 2;
11
const int32_t MINPROJECTION = (int32_t) MATH_UNIT_FACTOR * MATH_UNIT_FACTOR / 2;
12
 
12
 
13
// Takes 380 - 400 usec. Way too slow.
13
// Takes 380 - 400 usec. Way too slow.
Line 14... Line 14...
14
// With static MINPROJECTION: 220 usec.
14
// With static MINPROJECTION: 220 usec.
15
uint16_t AC_getThrottle(uint16_t throttle) {
15
uint16_t AC_getThrottle(uint16_t throttle) {
16
  int32_t projection;
16
        int32_t projection;
17
 
17
 
18
  // part1 start: 150 usec
18
        // part1 start: 150 usec
19
  // It's factor (int32_t)MATH_UNIT_FACTOR^2 too high.
19
        // It's factor (int32_t)MATH_UNIT_FACTOR^2 too high.
20
  projection = (int32_t)int_cos(angle[PITCH]) * (int32_t)int_cos(angle[ROLL]);
20
        projection = (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]);
21
  // part1 end.
21
        // part1 end.
22
 
22
 
23
  uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3
23
        uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3
24
  int16_t deltaThrottle;
24
        int16_t deltaThrottle;
25
 
25
 
26
  if (projection < MINPROJECTION && projection >= 0) {
26
        if (projection < MINPROJECTION && projection >= 0) {
27
    // projection = MINPROJECTION;
27
                // projection = MINPROJECTION;
28
    deltaThrottle = 0;
28
                deltaThrottle = 0;
29
  } else if (projection >- MINPROJECTION && projection<0) {
29
        } else if (projection > -MINPROJECTION && projection < 0) {
30
    // projection = -MINPROJECITON;
30
                // projection = -MINPROJECITON;
31
    deltaThrottle = 0;
31
                deltaThrottle = 0;
32
  } else
32
        } else
-
 
33
                /*
33
    /*
34
                 * We need delta throttle = constant/projection1
34
     * We need delta throttle = constant/projection1
35
                 * (constant * MATH_UNIT_FACTOR^2) / projection.
Line 35... Line 36...
35
     * (constant * MATH_UNIT_FACTOR^2) / projection.
36
                 */
36
     */
37
                deltaThrottle = ((int32_t) effect * (int32_t) MATH_UNIT_FACTOR
37
    deltaThrottle = ((int32_t)effect * (int32_t)MATH_UNIT_FACTOR * (int32_t)MATH_UNIT_FACTOR) / (projection / 10)  - effect * 10;
38
                                * (int32_t) MATH_UNIT_FACTOR) / (projection / 10) - effect * 10;
38
  // DebugOut.Analog[13] = deltaThrottle;
39
        // DebugOut.Analog[13] = deltaThrottle;
39
 
40
 
40
  return throttle + deltaThrottle;
41
        return throttle + deltaThrottle;
41
}
42
}