Subversion Repositories FlightCtrl

Rev

Rev 2069 | Rev 2117 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2069 Rev 2070
Line 6... Line 6...
6 
 
 6 
 
Line 7... Line 7...
7 
#define COARSERESOLUTION 1
 7 
#define COARSERESOLUTION 1
8 
 
 8 
 
- 
 
 9 
#ifdef COARSERESOLUTION
9 
#ifdef COARSERESOLUTION
 10 
#define NEUTRAL_PULSELENGTH 938
10 
#define NEUTRAL_PULSELENGTH 938
 11 
#define STABILIZATION_LOG_DIVIDER 6
11 
#define SERVOLIMIT 500
 12 
#define SERVOLIMIT 500
Line 12... Line 13...
12 
#define SCALE_FACTOR 4
 13 
#define SCALE_FACTOR 4
Line 13... Line 14...
13 
#define CS2 ((1<<CS21)|(1<<CS20))
 14 
#define CS2 ((1<<CS21)|(1<<CS20))
- 
 
 15 
 
14 
 
 16 
#else
15 
#else
 17 
 
16 
 
 18 
#define NEUTRAL_PULSELENGTH 3750
17 
#define NEUTRAL_PULSELENGTH 3750
 19 
#define STABILIZATION_LOG_DIVIDER 4
Line 103... Line 105...
103 
  uint8_t maxValue;
 105 
  uint8_t maxValue;
104 
  uint8_t flags;
 106 
  uint8_t flags;
105 
} servo_t;*/
 107 
} servo_t;*/
Line 106... Line 108...
106 
 
 108 
 
107 
int16_t calculateStabilizedServoAxis(uint8_t axis) {
 109 
int16_t calculateStabilizedServoAxis(uint8_t axis) {
108 
  int32_t value = attitude[axis] / 64L; // between -500000 to 500000 extreme limits. Just about
 110 
  int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about
109 
  // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000.
 111 
  // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000.
110 
  // That is a divisor of about 1<<14. Same conclusion as H&I.
 112 
  // That is a divisor of about 1<<14. Same conclusion as H&I.
111 
  value *= staticParams.servoConfigurations[axis].stabilizationFactor;
 113 
  value *= staticParams.servoConfigurations[axis].stabilizationFactor;
112 
  value /= 256L;
 114 
  value = value >> 8;
113 
  if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE)
 115 
  if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE)
114 
    return -value;
 116 
    return -value;
115 
  return value;
 117 
  return value;