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 
}