Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

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