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"
2048 - 6 
#include "controlMixer.h"
1775 - 7 
 
8 
// For scope debugging only!
1872 - 9 
#include "output.h"
1775 - 10 
 
11 
// = cos^2(45 degs).
1887 - 12 
// const int32_t FACTORSQUARED = 1L << (MATH_UNIT_FACTOR_LOG * 2);
2047 - 13 
const int32_t MINPROJECTION = 1L << (LOG_MATH_UNIT_FACTOR * 2 - 9);
1775 - 14 
 
15 
// Takes 380 - 400 usec. Way too slow.
16 
// With static MINPROJECTION: 220 usec.
2048 - 17 
void AC_getPRTY(int16_t* PRTY) {
18 
  int16_t throttle = PRTY[CONTROL_THROTTLE];
1877 - 19 
  int32_t projection;
1960 - 20 
  uint8_t effect = dynamicParams.attitudeControl; // Userparam 3
1887 - 21 
  int16_t deltaThrottle, y;
22 
 
2048 - 23 
  int16_t rollAngleInDegrees = attitude[ROLL]/GYRO_DEG_FACTOR_PITCHROLL;
24 
  int16_t pitchAngleInDegrees = attitude[PITCH]/GYRO_DEG_FACTOR_PITCHROLL;
2045 - 25 
 
26 
  projection = (int32_t) cos_360(pitchAngleInDegrees) * (int32_t) cos_360(rollAngleInDegrees);
1887 - 27 
  projection >>= 8;
28 
 
29 
  if (projection < 0) {
30 
    // Case not yet considered!
31 
    y = 0;
32 
  } else {
1877 - 33 
    if (projection < MINPROJECTION && projection >= 0) {
1887 - 34 
      projection = MINPROJECTION;
1877 - 35 
    } else if (projection > -MINPROJECTION && projection < 0) {
1887 - 36 
      projection = -MINPROJECTION;
1877 - 37 
    } else {
1887 - 38 
    }
2047 - 39 
    y = ((int32_t) throttle << (LOG_MATH_UNIT_FACTOR * 2 - 8)) / projection - throttle;
1887 - 40 
  }
41 
  deltaThrottle = ((int32_t)y * effect) >> 6;
1978 - 42 
  // debugOut.analog[8] = deltaThrottle;
2048 - 43 
  PRTY[CONTROL_THROTTLE] = throttle;
1877 - 44 
}