Rev 1872 | Rev 1955 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1872 | Rev 1908 | ||
---|---|---|---|
Line 58... | Line 58... | ||
58 | #include "configuration.h" |
58 | #include "configuration.h" |
59 | #include "attitude.h" |
59 | #include "attitude.h" |
60 | #include "commands.h" |
60 | #include "commands.h" |
61 | #include "output.h" |
61 | #include "output.h" |
Line 62... | Line 62... | ||
62 | 62 | ||
- | 63 | // uint16_t maxControl[2] = { 0, 0 }; |
|
63 | uint16_t maxControl[2] = { 0, 0 }; |
64 | uint16_t controlActivity = 0; |
64 | int16_t control[2] = { 0, 0 }; |
65 | int16_t controls[4] = { 0, 0, 0, 0 }; |
65 | int16_t controlYaw = 0, controlThrottle = 0; |
66 | //int16_t controlYaw = 0, controlThrottle = 0; |
Line 66... | Line 67... | ||
66 | uint8_t looping = 0; |
67 | uint8_t looping = 0; |
67 | 68 | ||
68 | // Internal variables for reading commands made with an R/C stick. |
69 | // Internal variables for reading commands made with an R/C stick. |
Line 134... | Line 135... | ||
134 | uint8_t ecQ = EC_getSignalQuality(); |
135 | uint8_t ecQ = EC_getSignalQuality(); |
135 | // This needs not be the only correct solution... |
136 | // This needs not be the only correct solution... |
136 | return rcQ > ecQ ? rcQ : ecQ; |
137 | return rcQ > ecQ ? rcQ : ecQ; |
137 | } |
138 | } |
Line -... | Line 139... | ||
- | 139 | ||
- | 140 | void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) { |
|
- | 141 | int16_t tmp = controls[index]; |
|
- | 142 | controls[index] = newValue; |
|
- | 143 | tmp -= newValue; |
|
- | 144 | tmp /= 2; |
|
- | 145 | tmp = tmp * tmp; |
|
- | 146 | // tmp += (newValue >= 0) ? newValue : -newValue; |
|
- | 147 | if (controlActivity + (uint16_t)tmp >= controlActivity) |
|
- | 148 | controlActivity += tmp; |
|
- | 149 | else controlActivity = 0xffff; |
|
- | 150 | } |
|
- | 151 | ||
- | 152 | #define CADAMPING 10 |
|
- | 153 | void dampenControlActivity(void) { |
|
- | 154 | uint32_t tmp = controlActivity; |
|
- | 155 | tmp *= ((1<<CADAMPING)-1); |
|
- | 156 | tmp >>= CADAMPING; |
|
- | 157 | controlActivity = tmp; |
|
- | 158 | } |
|
138 | 159 | ||
139 | /* |
160 | /* |
140 | * Update the variables indicating stick position from the sum of R/C, GPS and external control. |
161 | * Update the variables indicating stick position from the sum of R/C, GPS and external control. |
141 | */ |
162 | */ |
142 | void controlMixer_update(void) { |
163 | void controlMixer_update(void) { |
143 | // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
164 | // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
144 | // TODO: If no signal --> zero. |
- | |
145 | uint8_t axis; |
165 | // TODO: If no signal --> zero. |
Line 146... | Line 166... | ||
146 | int16_t tempThrottle; |
166 | int16_t tempThrottle; |
147 | 167 | ||
Line 152... | Line 172... | ||
152 | EC_update(); |
172 | EC_update(); |
Line 153... | Line 173... | ||
153 | 173 | ||
154 | // takes about 80 usec. |
174 | // takes about 80 usec. |
Line 155... | Line 175... | ||
155 | HC_update(); |
175 | HC_update(); |
156 | 176 | ||
Line 157... | Line 177... | ||
157 | int16_t* RC_PRTY = RC_getPRTY(); |
177 | int16_t* RC_PRTY = RC_getPRTY(); |
158 | int16_t* EC_PRTY = EC_getPRTY(); |
178 | int16_t* EC_PRTY = EC_getPRTY(); |
- | 179 | ||
- | 180 | updateControlAndMeasureControlActivity(CONTROL_PITCH, RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]); |
|
Line 159... | Line -... | ||
159 | - | ||
160 | control[PITCH] = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]; |
181 | updateControlAndMeasureControlActivity(CONTROL_ROLL, RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]); |
Line 161... | Line 182... | ||
161 | control[ROLL] = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]; |
182 | updateControlAndMeasureControlActivity(CONTROL_YAW, RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]); |
- | 183 | dampenControlActivity(); |
|
Line 162... | Line -... | ||
162 | - | ||
163 | tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]); |
184 | |
Line 164... | Line 185... | ||
164 | controlThrottle = AC_getThrottle(tempThrottle); |
185 | DebugOut.Analog[14] = controlActivity/10; |
165 | 186 | ||
166 | controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
187 | tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]); |
167 | 188 | controls[CONTROL_THROTTLE] = AC_getThrottle(tempThrottle); |
|
Line 188... | Line 209... | ||
188 | } |
209 | } |
189 | */ |
210 | */ |
Line 190... | Line 211... | ||
190 | 211 | ||
191 | /* |
212 | /* |
192 | * Record maxima |
- | |
193 | */ |
213 | * Record maxima |
194 | for (axis = PITCH; axis <= ROLL; axis++) { |
214 | for (axis = PITCH; axis <= ROLL; axis++) { |
195 | if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) { |
215 | if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) { |
196 | maxControl[axis] = abs(control[axis]) / CONTROL_SCALING; |
216 | maxControl[axis] = abs(control[axis]) / CONTROL_SCALING; |
197 | if (maxControl[axis] > 100) |
217 | if (maxControl[axis] > 100) |
198 | maxControl[axis] = 100; |
218 | maxControl[axis] = 100; |
199 | } else if (maxControl[axis]) |
219 | } else if (maxControl[axis]) |
200 | maxControl[axis]--; |
220 | maxControl[axis]--; |
- | 221 | } |
|
Line 201... | Line 222... | ||
201 | } |
222 | */ |
202 | 223 | ||
203 | uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() |
224 | uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() |
204 | : COMMAND_NONE; |
225 | : COMMAND_NONE; |