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).
11
const int32_t MINPROJECTION =  (int32_t)MATH_UNIT_FACTOR * MATH_UNIT_FACTOR / 2;
12
 
13
// Takes 380 - 400 usec. Way too slow.
14
// With static MINPROJECTION: 220 usec.
15
uint16_t AC_getThrottle(uint16_t throttle) {
16
  int32_t projection;
17
 
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.
22
 
23
  uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3
24
  int16_t deltaThrottle;
25
 
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 * (int32_t)MATH_UNIT_FACTOR) / (projection / 10)  - effect * 10;
38
  // DebugOut.Analog[13] = deltaThrottle;
39
 
40
  return throttle + deltaThrottle;
41
}
42
/*
43
har: R = e * k/p
44
vil  R = e * ( 1 - k/p )
45
       = e - ek/p
46
*/