Subversion Repositories FlightCtrl

Rev

Rev 2047 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2047 Rev 2048
Line 1... Line 1...
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
#include "controlMixer.h"
Line 6... Line 7...
6
 
7
 
7
// For scope debugging only!
8
// For scope debugging only!
Line 8... Line 9...
8
#include "output.h"
9
#include "output.h"
9
 
10
 
10
// = cos^2(45 degs).
11
// = cos^2(45 degs).
Line 11... Line 12...
11
// const int32_t FACTORSQUARED = 1L << (MATH_UNIT_FACTOR_LOG * 2);
12
// const int32_t FACTORSQUARED = 1L << (MATH_UNIT_FACTOR_LOG * 2);
12
const int32_t MINPROJECTION = 1L << (LOG_MATH_UNIT_FACTOR * 2 - 9);
13
const int32_t MINPROJECTION = 1L << (LOG_MATH_UNIT_FACTOR * 2 - 9);
13
 
14
 
-
 
15
// Takes 380 - 400 usec. Way too slow.
14
// Takes 380 - 400 usec. Way too slow.
16
// With static MINPROJECTION: 220 usec.
15
// With static MINPROJECTION: 220 usec.
17
void AC_getPRTY(int16_t* PRTY) {
16
uint16_t AC_getThrottle(uint16_t throttle) {
18
  int16_t throttle = PRTY[CONTROL_THROTTLE];
Line 17... Line 19...
17
  int32_t projection;
19
  int32_t projection;
18
  uint8_t effect = dynamicParams.attitudeControl; // Userparam 3
20
  uint8_t effect = dynamicParams.attitudeControl; // Userparam 3
Line 19... Line 21...
19
  int16_t deltaThrottle, y;
21
  int16_t deltaThrottle, y;
20
 
22
 
Line 21... Line 23...
21
  int16_t rollAngleInDegrees = angle[ROLL] / GYRO_DEG_FACTOR_PITCHROLL;
23
  int16_t rollAngleInDegrees = attitude[ROLL]/GYRO_DEG_FACTOR_PITCHROLL;
Line 36... Line 38...
36
    }
38
    }
37
    y = ((int32_t) throttle << (LOG_MATH_UNIT_FACTOR * 2 - 8)) / projection - throttle;
39
    y = ((int32_t) throttle << (LOG_MATH_UNIT_FACTOR * 2 - 8)) / projection - throttle;
38
  }
40
  }
39
  deltaThrottle = ((int32_t)y * effect) >> 6;
41
  deltaThrottle = ((int32_t)y * effect) >> 6;
40
  // debugOut.analog[8] = deltaThrottle;
42
  // debugOut.analog[8] = deltaThrottle;
41
  return throttle + deltaThrottle;
43
  PRTY[CONTROL_THROTTLE] = throttle;
42
}
44
}