Subversion Repositories FlightCtrl

Rev

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

Rev 2099 Rev 2102
Line 1... Line 1...
1 
#include <avr/io.h>
 1 
#include <avr/io.h>
2 
#include <avr/interrupt.h>
 2 
#include <avr/interrupt.h>
3 
#include "eeprom.h"
 3 
#include "eeprom.h"
- 
 
 4 
#include "output.h"
4 
#include "rc.h"
 5 
#include "flight.h"
5 
#include "attitude.h"
 6 
#include "attitude.h"
Line 6... Line 7...
6 
 
 7 
 
Line 7... Line 8...
7 
#define COARSERESOLUTION 1
 8 
// #define COARSERESOLUTION 1
8 
 
 9 
 
9 
#ifdef COARSERESOLUTION
 10 
#ifdef COARSERESOLUTION
10 
#define NEUTRAL_PULSELENGTH 938
 11 
#define NEUTRAL_PULSELENGTH 938
Line 24... Line 25...
24 
#define MAX_SERVOS 8
 25 
#define MAX_SERVOS 8
25 
#define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.servoCount + 128)
 26 
#define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.servoCount + 128)
26 
#define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT)
 27 
#define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT)
27 
#define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT)
 28 
#define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT)
Line 28... Line -...
28 
 
 - 
 
29 
//volatile uint8_t servoActive = 0;
 29 
 
30 
volatile uint8_t recalculateServoTimes = 0;
 30 
volatile uint8_t recalculateServoTimes = 0;
31 
volatile uint16_t servoValues[MAX_SERVOS];
 31 
volatile uint16_t servoValues[MAX_SERVOS];
Line 32... Line 32...
32 
volatile uint16_t previousManualValues[2];
 32 
volatile uint16_t previousManualValues[2];
Line 81... Line 81...
81 
      previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR;
 81 
      previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR;
Line 82... Line 82...
82 
 
 82 
 
83 
    SREG = sreg;
 83 
    SREG = sreg;
Line 84... Line -...
84 
}
 - 
 
85 
 
 - 
 
86 
/*
 - 
 
87 
void servo_On(void) {
 - 
 
88 
    servoActive = 1;
 - 
 
89 
}
 - 
 
90 
void servo_Off(void) {
 - 
 
91 
    servoActive = 0;
 - 
 
92 
    HEF4017R_ON; // enable reset
 - 
 
93 
}
 - 
 
94 
*/
 84 
}
95 
 
 85 
 
96 
/*****************************************************
 86 
/*****************************************************
97 
 * Control Servo Position
 - 
 
98 
 *****************************************************/
 - 
 
99 
 
 - 
 
100 
/*typedef struct {
 - 
 
101 
  uint8_t manualControl;
 - 
 
102 
  uint8_t compensationFactor;
 - 
 
103 
  uint8_t minValue;
 - 
 
104 
  uint8_t maxValue;
 - 
 
105 
  uint8_t flags;
 - 
 
106 
} servo_t;*/
 87 
 * Control (camera gimbal etc.) servos
107 
 
 88 
 *****************************************************/
108 
int16_t calculateStabilizedServoAxis(uint8_t axis) {
 89 
int16_t calculateStabilizedServoAxis(uint8_t axis) {
109 
  int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about
 90 
  int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about
110 
  // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000.
 91 
  // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000.
Line 134... Line 115...
134 
  value += calculateStabilizedServoAxis(axis);
 115 
  value += calculateStabilizedServoAxis(axis);
135 
  int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR;
 116 
  int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR;
136 
  if (value < limit) value = limit;
 117 
  if (value < limit) value = limit;
137 
  limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR;
 118 
  limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR;
138 
  if (value > limit) value = limit;
 119 
  if (value > limit) value = limit;
139 
  return value;
 - 
 
140 
}
 - 
 
141 
 
 - 
 
142 
uint16_t servoValue(uint8_t axis) {
 - 
 
143 
  int16_t value;
 - 
 
144 
  if (axis<2) value = featuredServoValue(axis);
 - 
 
145 
  else value = 128 * SCALE_FACTOR; // dummy. Replace by something useful for servos 3..8.
 - 
 
146 
  // Shift out of the [0..255*SCALE_FACTOR] space
 - 
 
147 
  value -= (128 * SCALE_FACTOR);
 120 
  value -= (128 * SCALE_FACTOR);
148 
  if (value < -SERVOLIMIT) value = -SERVOLIMIT;
 121 
  if (value < -SERVOLIMIT) value = -SERVOLIMIT;
149 
  else if (value > SERVOLIMIT) value = SERVOLIMIT;
 122 
  else if (value > SERVOLIMIT) value = SERVOLIMIT;
150 
  // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space.
 123 
  // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space.
151 
  return value + NEUTRAL_PULSELENGTH;
 124 
  return value + NEUTRAL_PULSELENGTH;
152 
}
 125 
}
Line 153... Line 126...
153 
 
 126 
 
- 
 
 127 
void calculateControlServoValues(void) {
- 
 
 128 
  int16_t value;
- 
 
 129 
  for (uint8_t axis=0; axis<3; axis++) {
- 
 
 130 
        value = controlServos[axis];
- 
 
 131 
        value /= 2;
- 
 
 132 
        servoValues[axis] = value + NEUTRAL_PULSELENGTH;
- 
 
 133 
  }
- 
 
 134 
  debugOut.analog[18] = servoValues[0];
- 
 
 135 
  debugOut.analog[19] = servoValues[2];
- 
 
 136 
}
- 
 
 137 
 
- 
 
 138 
void calculateFeaturedServoValues(void) {
- 
 
 139 
  int16_t value;
- 
 
 140 
  uint8_t axis;
- 
 
 141 
 
154 
void calculateServoValues(void) {
 142 
  // Save the computation cost of computing a new value before the old one is used.
- 
 
 143 
  if (!recalculateServoTimes) return;
155 
  if (!recalculateServoTimes) return;
 144 
 
- 
 
 145 
  for (axis=0; axis<2; axis++) {
156 
  for (uint8_t axis=0; axis<MAX_SERVOS; axis++) {
 146 
        value = featuredServoValue(axis);
157 
    servoValues[axis] = servoValue(axis);
 147 
        servoValues[axis + 3] = value;
- 
 
 148 
  }
- 
 
 149 
  for (axis=2; axis<MAX_SERVOS; axis++) {
- 
 
 150 
        value = 128 * SCALE_FACTOR;
- 
 
 151 
        servoValues[axis + 3] = value;
- 
 
 152 
  }
158 
  }
 153 
 
159 
  recalculateServoTimes = 0;
 154 
  recalculateServoTimes = 0;
Line 160... Line 155...
160 
}
 155 
}
161 
 
 156