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 |