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!
1872 - 8
#include "output.h"
1775 - 9
 
10
// = cos^2(45 degs).
1887 - 11
// const int32_t FACTORSQUARED = 1L << (MATH_UNIT_FACTOR_LOG * 2);
12
const int32_t MINPROJECTION = 1L << (MATH_UNIT_FACTOR_LOG * 2 - 9);
1775 - 13
 
14
// Takes 380 - 400 usec. Way too slow.
15
// With static MINPROJECTION: 220 usec.
16
uint16_t AC_getThrottle(uint16_t throttle) {
1877 - 17
  int32_t projection;
1960 - 18
  uint8_t effect = dynamicParams.attitudeControl; // Userparam 3
1887 - 19
  int16_t deltaThrottle, y;
20
 
21
  projection = (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]);
22
  projection >>= 8;
23
 
24
  if (projection < 0) {
25
    // Case not yet considered!
26
    y = 0;
27
  } else {
1877 - 28
    if (projection < MINPROJECTION && projection >= 0) {
1887 - 29
      projection = MINPROJECTION;
1877 - 30
    } else if (projection > -MINPROJECTION && projection < 0) {
1887 - 31
      projection = -MINPROJECTION;
1877 - 32
    } else {
1887 - 33
    }
34
    y = ((int32_t) throttle << (MATH_UNIT_FACTOR_LOG * 2 - 8)) / projection
35
        - throttle;
36
  }
37
  deltaThrottle = ((int32_t)y * effect) >> 6;
1955 - 38
  debugOut.analog[8] = deltaThrottle;
1877 - 39
  return throttle + deltaThrottle;
40
}