Rev 2045 | Rev 2051 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2045 | Rev 2048 | ||
---|---|---|---|
Line 1... | Line -... | ||
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
2 | // + Copyright (c) 04.2007 Holger Buss |
- | |
3 | // + Nur f�r den privaten Gebrauch |
- | |
4 | // + www.MikroKopter.com |
- | |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
6 | // + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation), |
- | |
7 | // + dass eine Nutzung (auch auszugsweise) nur f�r den privaten (nicht-kommerziellen) Gebrauch zul�ssig ist. |
- | |
8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
- | |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
- | |
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen, |
- | |
11 | // + Verkauf von Luftbildaufnahmen, usw. |
- | |
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
13 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht, |
- | |
14 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen |
- | |
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
- | |
17 | // + auf anderen Webseiten oder sonstigen Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
- | |
18 | // + eindeutig als Ursprung verlinkt werden |
- | |
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
20 | // + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion |
- | |
21 | // + Benutzung auf eigene Gefahr |
- | |
22 | // + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den |
- | |
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
24 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
- | |
25 | // + mit unserer Zustimmung zul�ssig |
- | |
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
- | |
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
- | |
30 | // + this list of conditions and the following disclaimer. |
- | |
31 | // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
- | |
32 | // + from this software without specific prior written permission. |
- | |
33 | // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
- | |
34 | // + for non-commercial use (directly or indirectly) |
- | |
35 | // + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
- | |
36 | // + with our written permission |
- | |
37 | // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
- | |
38 | // + clearly linked as origin |
- | |
39 | // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
- | |
40 | // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
- | |
41 | // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
- | |
42 | // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
- | |
43 | // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
- | |
44 | // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
- | |
45 | // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
- | |
46 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
- | |
47 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
- | |
48 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
- | |
49 | // + POSSIBILITY OF SUCH DAMAGE. |
- | |
50 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
51 | #include <stdlib.h> |
1 | #include <stdlib.h> |
52 | #include <avr/io.h> |
2 | #include <avr/io.h> |
53 | #include <avr/interrupt.h> |
3 | #include <avr/interrupt.h> |
Line 54... | Line 4... | ||
54 | 4 | ||
55 | #include "rc.h" |
5 | #include "rc.h" |
56 | #include "uart0.h" |
6 | #include "uart0.h" |
57 | #include "controlMixer.h" |
7 | #include "controlMixer.h" |
58 | #include "configuration.h" |
8 | #include "configuration.h" |
Line 59... | Line 9... | ||
59 | #include "commands.h" |
9 | #include "commands.h" |
60 | 10 | ||
61 | // The channel array is now 0-based. |
11 | // The channel array is 0-based! |
62 | volatile int16_t PPM_in[MAX_CHANNELS]; |
- | |
63 | volatile int16_t PPM_diff[MAX_CHANNELS]; |
12 | volatile int16_t PPM_in[MAX_CHANNELS]; |
64 | volatile uint8_t newPPMData = 1; |
- | |
65 | volatile uint8_t RCQuality; |
13 | volatile int16_t PPM_diff[MAX_CHANNELS]; |
66 | int16_t RC_PRTY[4]; |
14 | volatile uint8_t RCQuality; |
Line 67... | Line 15... | ||
67 | uint8_t lastRCCommand = COMMAND_NONE; |
15 | uint8_t lastRCCommand = COMMAND_NONE; |
68 | uint8_t commandTimer = 0; |
16 | uint8_t commandTimer = 0; |
Line 138... | Line 86... | ||
138 | The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms. |
86 | The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms. |
139 | The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms. |
87 | The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms. |
140 | The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is |
88 | The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is |
141 | the syncronization gap. |
89 | the syncronization gap. |
142 | */ |
90 | */ |
143 | ISR(TIMER1_CAPT_vect) |
- | |
144 | { // typical rate of 1 ms to 2 ms |
91 | ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms |
145 | int16_t signal = 0, tmp; |
92 | int16_t signal = 0, tmp; |
146 | static int16_t index; |
93 | static int16_t index; |
147 | static uint16_t oldICR1 = 0; |
94 | static uint16_t oldICR1 = 0; |
Line 148... | Line 95... | ||
148 | 95 | ||
Line 155... | Line 102... | ||
155 | signal = (uint16_t) ICR1 - oldICR1; |
102 | signal = (uint16_t) ICR1 - oldICR1; |
156 | oldICR1 = ICR1; |
103 | oldICR1 = ICR1; |
Line 157... | Line 104... | ||
157 | 104 | ||
158 | //sync gap? (3.52 ms < signal < 25.6 ms) |
105 | //sync gap? (3.52 ms < signal < 25.6 ms) |
159 | if ((signal > 1100) && (signal < 8000)) { |
- | |
160 | // if a sync gap happens and there where at least 4 channels decoded before |
- | |
161 | // then the NewPpmData flag is reset indicating valid data in the PPM_in[] array. |
- | |
162 | if (index >= 3) { |
- | |
163 | newPPMData = 0; // Null means NewData for the first 4 channels |
- | |
164 | } |
- | |
165 | // synchronize channel index |
106 | if ((signal > 1100) && (signal < 8000)) { |
166 | index = 0; |
107 | index = 0; |
167 | } else { // within the PPM frame |
108 | } else { // within the PPM frame |
168 | if (index < MAX_CHANNELS) { // PPM24 supports 12 channels |
109 | if (index < MAX_CHANNELS) { // PPM24 supports 12 channels |
169 | // check for valid signal length (0.8 ms < signal < 2.1984 ms) |
110 | // check for valid signal length (0.8 ms < signal < 2.1984 ms) |
Line 235... | Line 176... | ||
235 | // vertical is around center |
176 | // vertical is around center |
236 | return COMMAND_NONE; |
177 | return COMMAND_NONE; |
237 | } |
178 | } |
Line 238... | Line 179... | ||
238 | 179 | ||
239 | /* |
180 | /* |
240 | * This must be called (as the only thing) for each control loop cycle (488 Hz). |
181 | * Get Pitch, Roll, Throttle, Yaw values |
241 | */ |
182 | */ |
242 | void RC_periodicTask() { |
183 | void RC_periodicTaskAndPRTY(int16_t* PRTY) { |
243 | int16_t tmp1, tmp2; |
184 | int16_t tmp1, tmp2; |
244 | if (RCQuality) { |
185 | if (RCQuality) { |
245 | RCQuality--; |
- | |
246 | if (newPPMData-- == 0) { |
186 | RCQuality--; |
247 | RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP |
- | |
248 | + RCDiff(CH_PITCH) * staticParams.stickD; |
187 | PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD; |
249 | RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP |
- | |
250 | + RCDiff(CH_ROLL) * staticParams.stickD; |
188 | PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD; |
251 | RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) |
- | |
252 | * staticParams.stickThrottleD + 120; |
- | |
253 | if (RC_PRTY[CONTROL_THROTTLE] < 0) |
189 | PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + 120; |
254 | RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
190 | if (PRTY[CONTROL_THROTTLE] < 0) PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
255 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
191 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
256 | // exponential stick sensitivity in yawing rate |
192 | // exponential stick sensitivity in yawing rate |
257 | tmp2 = (int32_t) staticParams.stickYawP * ((int32_t) tmp1 * abs(tmp1)) |
- | |
258 | / 512L; // expo y = ax + bx^2 |
193 | tmp2 = (int32_t) staticParams.stickYawP * ((int32_t) tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2 |
259 | tmp2 += (staticParams.stickYawP * tmp1) >> 2; |
194 | tmp2 += (staticParams.stickYawP * tmp1) >> 2; |
260 | RC_PRTY[CONTROL_YAW] = tmp2; |
- | |
261 | } |
- | |
Line -... | Line 195... | ||
- | 195 | PRTY[CONTROL_YAW] = tmp2; |
|
262 | uint8_t command = RC_getStickCommand(); |
196 | |
263 | 197 | uint8_t command = RC_getStickCommand(); |
|
264 | if (lastRCCommand == command) { |
198 | if (lastRCCommand == command) { |
265 | // Keep timer from overrunning. |
199 | // Keep timer from overrunning. |
266 | if (commandTimer < COMMAND_TIMER) |
200 | if (commandTimer < COMMAND_TIMER) |
267 | commandTimer++; |
201 | commandTimer++; |
268 | } else { |
202 | } else { |
269 | // There was a change. |
203 | // There was a change. |
270 | lastRCCommand = command; |
204 | lastRCCommand = command; |
271 | commandTimer = 0; |
- | |
272 | } |
- | |
273 | } else { // Bad signal |
205 | commandTimer = 0; |
274 | RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] = RC_PRTY[CONTROL_YAW] = 0; |
206 | } |
275 | } |
207 | } |
Line 276... | Line 208... | ||
276 | debugOut.analog[18] = RCQuality; |
208 | debugOut.analog[18] = RCQuality; |
277 | } |
- | |
278 | - | ||
279 | /* |
- | |
280 | * Get Pitch, Roll, Throttle, Yaw values |
- | |
281 | */ |
- | |
282 | int16_t* RC_getPRTY(void) { |
- | |
283 | return RC_PRTY; |
- | |
284 | } |
209 | } |
285 | 210 | ||
286 | /* |
211 | /* |
287 | * Get other channel value |
212 | * Get other channel value |
288 | */ |
213 | */ |
Line 382... | Line 307... | ||
382 | return 0; |
307 | return 0; |
383 | } |
308 | } |
384 | } |
309 | } |
Line 385... | Line 310... | ||
385 | 310 | ||
386 | /* |
- | |
387 | uint8_t RC_getLooping(uint8_t looping) { |
- | |
388 | // static uint8_t looping = 0; |
- | |
389 | - | ||
390 | if (RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig |
- | |
391 | & CFG_LOOP_LEFT) { |
- | |
392 | looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT); |
- | |
393 | } else if ((looping & LOOPING_LEFT) && RCChannel(CH_ROLL) |
- | |
394 | < staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
- | |
395 | looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT)); |
- | |
396 | } |
- | |
397 | - | ||
398 | if (RCChannel(CH_ROLL) < -staticParams.LoopThreshold |
311 | /* |
399 | && staticParams.BitConfig & CFG_LOOP_RIGHT) { |
- | |
400 | looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT); |
- | |
401 | } else if ((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL) |
- | |
402 | > -staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
- | |
403 | looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT)); |
- | |
404 | } |
- | |
405 | - | ||
406 | if (RCChannel(CH_PITCH) > staticParams.LoopThreshold |
- | |
407 | && staticParams.BitConfig & CFG_LOOP_UP) { |
- | |
408 | looping |= (LOOPING_PITCH_AXIS | LOOPING_UP); |
- | |
409 | } else if ((looping & LOOPING_UP) && RCChannel(CH_PITCH) |
- | |
410 | < staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
- | |
411 | looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP)); |
- | |
412 | } |
- | |
413 | - | ||
414 | if (RCChannel(CH_PITCH) < -staticParams.LoopThreshold |
- | |
415 | && staticParams.BitConfig & CFG_LOOP_DOWN) { |
- | |
416 | looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN); |
- | |
417 | } else if ((looping & LOOPING_DOWN) && RCChannel(CH_PITCH) |
- | |
418 | > -staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
- | |
419 | looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN)); |
- | |
420 | } |
- | |
421 | - | ||
422 | return looping; |
- | |
423 | } |
312 | * For each time the stick is pulled, returns true. |
424 | */ |
- | |
425 | 313 | */ |
|
426 | uint8_t RC_testCompassCalState(void) { |
314 | uint8_t RC_testCompassCalState(void) { |
427 | static uint8_t stick = 1; |
315 | static uint8_t stickPulled = 1; |
428 | // if pitch is centered or top set stick to zero |
316 | // if pitch is centered or top set stick to zero |
429 | if (RCChannel(CH_PITCH) > -20) |
317 | if (RCChannel(CH_PITCH) > -20) |
430 | stick = 0; |
318 | stickPulled = 0; |
431 | // if pitch is down trigger to next cal state |
319 | // if pitch is down trigger to next cal state |
432 | if ((RCChannel(CH_PITCH) < -70) && !stick) { |
320 | if ((RCChannel(CH_PITCH) < -70) && !stickPulled) { |
433 | stick = 1; |
321 | stickPulled = 1; |
434 | return 1; |
322 | return 1; |
435 | } |
323 | } |
436 | return 0; |
324 | return 0; |
437 | } |
325 | } |