Subversion Repositories FlightCtrl

Rev

Rev 1872 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1872 Rev 1877
1 
#include <inttypes.h>
 1 
#include <inttypes.h>
2 
#include "attitude.h"
 2 
#include "attitude.h"
3 
#include "uart0.h"
 3 
#include "uart0.h"
4 
#include "configuration.h"
 4 
#include "configuration.h"
5 
#include "dongfangMath.h"
 5 
#include "dongfangMath.h"
6 
 
 6 
 
7 
// For scope debugging only!
 7 
// For scope debugging only!
8 
#include "output.h"
 8 
#include "output.h"
9 
 
 9 
 
10 
// = cos^2(45 degs).
 10 
// = cos^2(45 degs).
- 
 
 11 
const int32_t FACTORSQUARED = (int32_t) (1<<(MATH_UNIT_FACTOR_LOG*2));
11 
const int32_t MINPROJECTION = (int32_t) MATH_UNIT_FACTOR * MATH_UNIT_FACTOR / 2;
 12 
const int32_t MINPROJECTION = (int32_t) (1<<(MATH_UNIT_FACTOR_LOG*2-1));
12 
 
 13 
 
13 
// Takes 380 - 400 usec. Way too slow.
 14 
// Takes 380 - 400 usec. Way too slow.
14 
// With static MINPROJECTION: 220 usec.
 15 
// With static MINPROJECTION: 220 usec.
15 
uint16_t AC_getThrottle(uint16_t throttle) {
 16 
uint16_t AC_getThrottle(uint16_t throttle) {
16 
        int32_t projection;
 17 
  int32_t projection;
17 
    uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3
 18 
  uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3
18 
    int16_t deltaThrottle;
 19 
  int16_t deltaThrottle;
19 
 
 20 
 
20 
        // part1 start: 150 usec
 - 
 
21 
        // It's factor (int32_t)MATH_UNIT_FACTOR^2 too high.
 21 
  projection =  (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]);
22 
        projection = 0;
 - 
 
23 
 
 22 
 
24 
        // (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]);
 - 
 
25 
        // part1 end.
 23 
  /*
26 
        if (projection < MINPROJECTION && projection >= 0) {
 24 
    if (projection < MINPROJECTION && projection >= 0) {
27 
                // projection = MINPROJECTION;
 - 
 
28 
                deltaThrottle = 0;
 25 
    deltaThrottle = 0;
29 
        } else if (projection > -MINPROJECTION && projection < 0) {
 26 
    } else if (projection > -MINPROJECTION && projection < 0) {
30 
                // projection = -MINPROJECITON;
 27 
    // projection = -MINPROJECITON;
31 
                deltaThrottle = 0;
 28 
    deltaThrottle = 0;
32 
        } else {
 29 
    } else {
33 
                /*
 30 
  */
34 
                 * We need delta throttle = constant/projection1
 - 
 
35 
                 * (constant * MATH_UNIT_FACTOR^2) / projection.
 - 
 
36 
                 */
 31 
 
37 
                deltaThrottle = ((int32_t) effect * (int32_t) MATH_UNIT_FACTOR
 32 
  deltaThrottle = (((int32_t) effect * throttle) << (MATH_UNIT_FACTOR*2)) / (projection * 100) - (1 << (MATH_UNIT_FACTOR*2));
38 
                                * (int32_t) MATH_UNIT_FACTOR) / (projection / 10) - effect * 10;
 - 
 
39 
        // DebugOut.Analog[13] = deltaThrottle;
 33 
  // DebugOut.Analog[13] = deltaThrottle;
40 
        }
 - 
 
41 
 
 34 
 
42 
        return throttle + deltaThrottle;
 35 
  return throttle + deltaThrottle;
43 
}
 36 
}
44 
/*
 37 
 
45 
 har: R = e * k/p
 - 
 
46 
 vil  R = e * ( 1 - k/p )
 - 
 
47 
 = e - ek/p
 - 
 
48 
 */
 - 
 
49 
 
 38