Rev 1623 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1623 | Rev 1645 | ||
---|---|---|---|
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | // + Copyright (c) 04.2007 Holger Buss |
2 | // + Copyright (c) 04.2007 Holger Buss |
3 | // + Nur für den privaten Gebrauch |
3 | // + Nur für den privaten Gebrauch |
4 | // + www.MikroKopter.com |
4 | // + www.MikroKopter.com |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
6 | // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
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. |
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 |
8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
11 | // + Verkauf von Luftbildaufnahmen, usw. |
11 | // + Verkauf von Luftbildaufnahmen, usw. |
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
13 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
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 |
14 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
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" |
17 | // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
18 | // + eindeutig als Ursprung verlinkt werden |
18 | // + eindeutig als Ursprung verlinkt werden |
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
20 | // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
20 | // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
21 | // + Benutzung auf eigene Gefahr |
21 | // + Benutzung auf eigene Gefahr |
22 | // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
22 | // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
24 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
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 |
25 | // + mit unserer Zustimmung zulässig |
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
30 | // + this list of conditions and the following disclaimer. |
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 |
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. |
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 |
33 | // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
34 | // + for non-commercial use (directly or indirectly) |
34 | // + for non-commercial use (directly or indirectly) |
35 | // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
35 | // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
36 | // + with our written permission |
36 | // + with our written permission |
37 | // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
37 | // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
38 | // + clearly linked as origin |
38 | // + clearly linked as origin |
39 | // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
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" |
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 |
41 | // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
42 | // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
42 | // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
43 | // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
43 | // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
44 | // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
44 | // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
45 | // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
45 | // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
46 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
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) |
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 |
48 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
49 | // + POSSIBILITY OF SUCH DAMAGE. |
49 | // + POSSIBILITY OF SUCH DAMAGE. |
50 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
50 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
51 | 51 | ||
52 | /* |
52 | /* |
53 | OBSOLETED BY controlMixer.c. But this is how it looked - maybe somebody will find it simpler? |
53 | OBSOLETED BY controlMixer.c. But this is how it looked - maybe somebody will find it simpler? |
54 | 54 | ||
55 | #include <stdlib.h> |
55 | #include <stdlib.h> |
56 | #include "control.h" |
56 | #include "control.h" |
57 | 57 | ||
58 | #include "rc.h" |
58 | #include "rc.h" |
59 | #include "configuration.h" |
59 | #include "configuration.h" |
60 | #include "attitude.h" |
60 | #include "attitude.h" |
61 | #include "eeprom.h" |
61 | #include "eeprom.h" |
62 | #include "flight.h" |
62 | #include "flight.h" |
63 | 63 | ||
64 | #define RCChannel(dimension) (PPM_in[staticParams.ChannelAssignment[dimension]]) |
64 | #define RCChannel(dimension) (PPM_in[staticParams.ChannelAssignment[dimension]]) |
65 | 65 | ||
66 | uint16_t maxStickPitch = 0, maxStickRoll = 0; |
66 | uint16_t maxStickPitch = 0, maxStickRoll = 0; |
67 | int16_t stickPitch = 0, stickRoll = 0, stickYaw = 0, stickThrottle = 0; |
67 | int16_t stickPitch = 0, stickRoll = 0, stickYaw = 0, stickThrottle = 0; |
68 | int16_t GPSStickPitch = 0, GPSStickRoll = 0; |
68 | int16_t GPSStickPitch = 0, GPSStickRoll = 0; |
69 | int16_t externalStickPitch = 0, externalStickRoll = 0, externalStickYaw = 0, externalHeightValue = -20; |
69 | int16_t externalStickPitch = 0, externalStickRoll = 0, externalStickYaw = 0, externalHeightValue = -20; |
70 | 70 | ||
71 | // dongfang's own experiment: Cablibrated sticks. |
71 | // dongfang's own experiment: Cablibrated sticks. |
72 | int16_t stickOffsetPitch = 0, stickOffsetRoll = 0; |
72 | int16_t stickOffsetPitch = 0, stickOffsetRoll = 0; |
73 | 73 | ||
74 | // Looping-or-not flags. |
74 | // Looping-or-not flags. |
75 | uint8_t loopingPitch = 0, loopingRoll = 0; |
75 | uint8_t loopingPitch = 0, loopingRoll = 0; |
76 | uint8_t loopingLeft = 0, loopingRight = 0, loopingDown = 0, loopingTop = 0; |
76 | uint8_t loopingLeft = 0, loopingRight = 0, loopingDown = 0, loopingTop = 0; |
77 | 77 | ||
78 | // Internal variables for reading commands made with an R/S stick. |
78 | // Internal variables for reading commands made with an R/S stick. |
79 | uint8_t lastStickCommand = STICK_COMMAND_UNDEF; |
79 | uint8_t lastStickCommand = STICK_COMMAND_UNDEF; |
80 | uint8_t stickCommandTimer = 0; |
80 | uint8_t stickCommandTimer = 0; |
81 | 81 | ||
82 | ExternalControl_t externalControl; |
82 | ExternalControl_t externalControl; |
83 | 83 | ||
84 | / * |
84 | / * |
85 | * Stick diagram: |
85 | * Stick diagram: |
86 | * 2--3--4 |
86 | * 2--3--4 |
87 | * | | + |
87 | * | | + |
88 | * 1 9 5 ^ 0 |
88 | * 1 9 5 ^ 0 |
89 | * | | | |
89 | * | | | |
90 | * 8--7--6 |
90 | * 8--7--6 |
91 | * |
91 | * |
92 | * + <-- |
92 | * + <-- |
93 | * 0 |
93 | * 0 |
94 | * |
94 | * |
95 | * Not in any of these positions: 0 |
95 | * Not in any of these positions: 0 |
96 | * / |
96 | * / |
97 | 97 | ||
98 | / * |
98 | / * |
99 | * The stick most be further from center than this to indicate a settings number (1-5). |
99 | * The stick most be further from center than this to indicate a settings number (1-5). |
100 | * / |
100 | * / |
101 | #define STICK_SETTINGSELECTION_THRESHOLD 70 |
101 | #define STICK_SETTINGSELECTION_THRESHOLD 70 |
102 | 102 | ||
103 | uint8_t control_getLeftRCStickIndex(int16_t thresholdThrottle, int16_t thresholdYaw) { |
103 | uint8_t control_getLeftRCStickIndex(int16_t thresholdThrottle, int16_t thresholdYaw) { |
104 | if(RCChannel(CH_THROTTLE) > thresholdThrottle) { |
104 | if(RCChannel(CH_THROTTLE) > thresholdThrottle) { |
105 | // throttle is up |
105 | // throttle is up |
106 | if(RCChannel(CH_YAW) > thresholdYaw) |
106 | if(RCChannel(CH_YAW) > thresholdYaw) |
107 | return STICK_COMMAND_GYROCAL; |
107 | return STICK_COMMAND_GYROCAL; |
108 | if(RCChannel(CH_YAW) < -thresholdYaw) |
108 | if(RCChannel(CH_YAW) < -thresholdYaw) |
109 | return STICK_COMMAND_ACCCAL; |
109 | return STICK_COMMAND_ACCCAL; |
110 | return STICK_COMMAND_UNDEF; |
110 | return STICK_COMMAND_UNDEF; |
111 | } else if(RCChannel(CH_THROTTLE) < -thresholdThrottle) { |
111 | } else if(RCChannel(CH_THROTTLE) < -thresholdThrottle) { |
112 | // pitch is down |
112 | // pitch is down |
113 | if(RCChannel(CH_YAW) > thresholdYaw) |
113 | if(RCChannel(CH_YAW) > thresholdYaw) |
114 | return STICK_COMMAND_STOP; |
114 | return STICK_COMMAND_STOP; |
115 | if(RCChannel(CH_YAW) < -thresholdYaw) |
115 | if(RCChannel(CH_YAW) < -thresholdYaw) |
116 | return STICK_COMMAND_START; |
116 | return STICK_COMMAND_START; |
117 | return STICK_COMMAND_UNDEF; |
117 | return STICK_COMMAND_UNDEF; |
118 | } else { |
118 | } else { |
119 | // pitch is around center |
119 | // pitch is around center |
120 | return STICK_COMMAND_UNDEF; |
120 | return STICK_COMMAND_UNDEF; |
121 | } |
121 | } |
122 | } |
122 | } |
123 | 123 | ||
124 | uint8_t control_getRightRCStickIndex(void) { |
124 | uint8_t control_getRightRCStickIndex(void) { |
125 | if(RCChannel(CH_PITCH) > STICK_SETTINGSELECTION_THRESHOLD) { |
125 | if(RCChannel(CH_PITCH) > STICK_SETTINGSELECTION_THRESHOLD) { |
126 | // pitch is up |
126 | // pitch is up |
127 | if(RCChannel(CH_ROLL) > STICK_SETTINGSELECTION_THRESHOLD) |
127 | if(RCChannel(CH_ROLL) > STICK_SETTINGSELECTION_THRESHOLD) |
128 | return 2; |
128 | return 2; |
129 | if(RCChannel(CH_ROLL) < -STICK_SETTINGSELECTION_THRESHOLD) |
129 | if(RCChannel(CH_ROLL) < -STICK_SETTINGSELECTION_THRESHOLD) |
130 | return 4; |
130 | return 4; |
131 | return 3; |
131 | return 3; |
132 | } else if(RCChannel(CH_PITCH) < -STICK_SETTINGSELECTION_THRESHOLD) { |
132 | } else if(RCChannel(CH_PITCH) < -STICK_SETTINGSELECTION_THRESHOLD) { |
133 | // pitch is down |
133 | // pitch is down |
134 | if(RCChannel(CH_ROLL) > STICK_SETTINGSELECTION_THRESHOLD) |
134 | if(RCChannel(CH_ROLL) > STICK_SETTINGSELECTION_THRESHOLD) |
135 | return 8; |
135 | return 8; |
136 | if(RCChannel(CH_ROLL) < -STICK_SETTINGSELECTION_THRESHOLD) |
136 | if(RCChannel(CH_ROLL) < -STICK_SETTINGSELECTION_THRESHOLD) |
137 | return 6; |
137 | return 6; |
138 | return 7; |
138 | return 7; |
139 | } else { |
139 | } else { |
140 | // pitch is around center |
140 | // pitch is around center |
141 | if(RCChannel(CH_ROLL) > STICK_SETTINGSELECTION_THRESHOLD) |
141 | if(RCChannel(CH_ROLL) > STICK_SETTINGSELECTION_THRESHOLD) |
142 | return 1; |
142 | return 1; |
143 | if(RCChannel(CH_ROLL) < -STICK_SETTINGSELECTION_THRESHOLD) |
143 | if(RCChannel(CH_ROLL) < -STICK_SETTINGSELECTION_THRESHOLD) |
144 | return 5; |
144 | return 5; |
145 | return 9; |
145 | return 9; |
146 | } |
146 | } |
147 | } |
147 | } |
148 | 148 | ||
149 | / * |
149 | / * |
150 | * This could be expanded to take calibrate / start / stop commands from ohter sources |
150 | * This could be expanded to take calibrate / start / stop commands from ohter sources |
151 | * than the R/C (read: Custom MK R/C project) |
151 | * than the R/C (read: Custom MK R/C project) |
152 | * / |
152 | * / |
153 | void control_senseStickCommands(void) { |
153 | void control_senseStickCommands(void) { |
154 | uint8_t stickCommandNow = control_getLeftRCStickIndex(85, 85); |
154 | uint8_t stickCommandNow = control_getLeftRCStickIndex(85, 85); |
155 | if (stickCommandNow != lastStickCommand) { |
155 | if (stickCommandNow != lastStickCommand) { |
156 | lastStickCommand = stickCommandNow; |
156 | lastStickCommand = stickCommandNow; |
157 | stickCommandTimer = 0; |
157 | stickCommandTimer = 0; |
158 | } else { |
158 | } else { |
159 | if (stickCommandTimer < 201) |
159 | if (stickCommandTimer < 201) |
160 | stickCommandTimer++; |
160 | stickCommandTimer++; |
161 | } |
161 | } |
162 | } |
162 | } |
163 | 163 | ||
164 | / * |
164 | / * |
165 | * This could be expanded to take calibrate / start / stop commands from ohter sources |
165 | * This could be expanded to take calibrate / start / stop commands from ohter sources |
166 | * than the R/C (read: Custom MK R/C project) |
166 | * than the R/C (read: Custom MK R/C project) |
167 | * / |
167 | * / |
168 | uint8_t control_getStickCommand(void) { |
168 | uint8_t control_getStickCommand(void) { |
169 | // If the same command was made 200 times, it's stable. |
169 | // If the same command was made 200 times, it's stable. |
170 | if (stickCommandTimer >= 200) { |
170 | if (stickCommandTimer >= 200) { |
171 | return lastStickCommand; |
171 | return lastStickCommand; |
172 | } |
172 | } |
173 | return STICK_COMMAND_UNDEF; |
173 | return STICK_COMMAND_UNDEF; |
174 | } |
174 | } |
175 | 175 | ||
176 | uint8_t control_isStickCommandRepeated(void) { |
176 | uint8_t control_isStickCommandRepeated(void) { |
177 | return stickCommandTimer > 200 ? 1 : 0; |
177 | return stickCommandTimer > 200 ? 1 : 0; |
178 | } |
178 | } |
179 | 179 | ||
180 | / * |
180 | / * |
181 | * To be fired only when the right stick is in the center position. |
181 | * To be fired only when the right stick is in the center position. |
182 | * This will cause the value of pitch and roll stick to be adjusted |
182 | * This will cause the value of pitch and roll stick to be adjusted |
183 | * to zero (not just to near zero, as per the assumption in rc.c |
183 | * to zero (not just to near zero, as per the assumption in rc.c |
184 | * about the rc signal. I had values about 50..70 with a Futaba |
184 | * about the rc signal. I had values about 50..70 with a Futaba |
185 | * R617 receiver.) This calibration is not strictly necessary, but |
185 | * R617 receiver.) This calibration is not strictly necessary, but |
186 | * for control logic that depends on the exact (non)center position |
186 | * for control logic that depends on the exact (non)center position |
187 | * of a stick, it may be useful. |
187 | * of a stick, it may be useful. |
188 | * / |
188 | * / |
189 | void control_setNeutral(void) { |
189 | void control_setNeutral(void) { |
190 | stickOffsetPitch += stickPitch; |
190 | stickOffsetPitch += stickPitch; |
191 | stickOffsetRoll += stickRoll; |
191 | stickOffsetRoll += stickRoll; |
192 | } |
192 | } |
193 | 193 | ||
194 | / * |
194 | / * |
195 | * Set the potientiometer values to the values of the respective R/C channel |
195 | * Set the potientiometer values to the values of the respective R/C channel |
196 | * right now. No slew rate limit. |
196 | * right now. No slew rate limit. |
197 | * / |
197 | * / |
198 | void control_initPots(void) { |
198 | void control_initPots(void) { |
199 | uint8_t i; |
199 | uint8_t i; |
200 | for (i=0; i<4; i++) { |
200 | for (i=0; i<4; i++) { |
201 | pots[i] = RCChannel(CH_POTS + i) + POT_OFFSET; |
201 | pots[i] = RCChannel(CH_POTS + i) + POT_OFFSET; |
202 | } |
202 | } |
203 | for (i=4; i<8; i++) { |
203 | for (i=4; i<8; i++) { |
204 | pots[i] = PPM_in[9 + (i-4)] + POT_OFFSET; |
204 | pots[i] = PPM_in[9 + (i-4)] + POT_OFFSET; |
205 | } |
205 | } |
206 | } |
206 | } |
207 | 207 | ||
208 | / * |
208 | / * |
209 | * Update potentiometer values with slow slew rate. Could be made faster if desired. |
209 | * Update potentiometer values with slow slew rate. Could be made faster if desired. |
210 | * / |
210 | * / |
211 | void control_updatePots(void) { |
211 | void control_updatePots(void) { |
212 | uint8_t i; |
212 | uint8_t i; |
213 | uint16_t targetvalue; |
213 | uint16_t targetvalue; |
214 | for (i=0; i<8; i++) { |
214 | for (i=0; i<8; i++) { |
215 | if (i<4) // configured pots |
215 | if (i<4) // configured pots |
216 | targetvalue = RCChannel(CH_POTS + i) + POT_OFFSET; |
216 | targetvalue = RCChannel(CH_POTS + i) + POT_OFFSET; |
217 | else // PPM24-Extension |
217 | else // PPM24-Extension |
218 | targetvalue = PPM_in[9 + i] + POT_OFFSET; |
218 | targetvalue = PPM_in[9 + i] + POT_OFFSET; |
219 | if (targetvalue < 0) targetvalue = 0; |
219 | if (targetvalue < 0) targetvalue = 0; |
220 | if (pots[i] < targetvalue && pots[i] < 255) pots[i]++; else if(pots[i] > 0 && pots[i] > targetvalue) pots[i]--; |
220 | if (pots[i] < targetvalue && pots[i] < 255) pots[i]++; else if(pots[i] > 0 && pots[i] > targetvalue) pots[i]--; |
221 | } |
221 | } |
222 | } |
222 | } |
223 | 223 | ||
224 | / * |
224 | / * |
225 | * Update the variables indicating stick position from the sum of R/C, GPS and external control. |
225 | * Update the variables indicating stick position from the sum of R/C, GPS and external control. |
226 | * / |
226 | * / |
227 | void control_update(void) { |
227 | void control_update(void) { |
228 | // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
228 | // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
229 | stickPitch = RCChannel(CH_PITCH) * staticParams.StickP; |
229 | stickPitch = RCChannel(CH_PITCH) * staticParams.StickP; |
230 | // (stick_pitch * 3 + RCChannel(CH_PITCH) * staticParams.StickP) / 4; |
230 | // (stick_pitch * 3 + RCChannel(CH_PITCH) * staticParams.StickP) / 4; |
231 | stickPitch += PPM_diff[staticParams.ChannelAssignment[CH_PITCH]] * staticParams.StickD; |
231 | stickPitch += PPM_diff[staticParams.ChannelAssignment[CH_PITCH]] * staticParams.StickD; |
232 | stickPitch = stickPitch - stickOffsetPitch - GPSStickPitch; |
232 | stickPitch = stickPitch - stickOffsetPitch - GPSStickPitch; |
233 | |
233 | |
234 | stickRoll = RCChannel(CH_ROLL) * staticParams.StickP; |
234 | stickRoll = RCChannel(CH_ROLL) * staticParams.StickP; |
235 | // stick_roll = (stick_roll * 3 + RCChannel(CH_ROLL) * staticParams.StickP) / 4; |
235 | // stick_roll = (stick_roll * 3 + RCChannel(CH_ROLL) * staticParams.StickP) / 4; |
236 | stickRoll += PPM_diff[staticParams.ChannelAssignment[CH_ROLL]] * staticParams.StickD; |
236 | stickRoll += PPM_diff[staticParams.ChannelAssignment[CH_ROLL]] * staticParams.StickD; |
237 | stickRoll = stickRoll - stickOffsetRoll - GPSStickRoll; |
237 | stickRoll = stickRoll - stickOffsetRoll - GPSStickRoll; |
238 | |
238 | |
239 | // mapping of yaw |
239 | // mapping of yaw |
240 | stickYaw = -RCChannel(CH_YAW); |
240 | stickYaw = -RCChannel(CH_YAW); |
241 | // (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction) |
241 | // (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction) |
242 | if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) { |
242 | if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) { |
243 | if (stickYaw > 2) stickYaw-= 2; |
243 | if (stickYaw > 2) stickYaw-= 2; |
244 | else if (stickYaw< -2) stickYaw += 2; |
244 | else if (stickYaw< -2) stickYaw += 2; |
245 | else stickYaw = 0; |
245 | else stickYaw = 0; |
246 | } |
246 | } |
247 | |
247 | |
248 | // mapping of gas |
248 | // mapping of gas |
249 | stickThrottle = RCChannel(CH_THROTTLE) + 120;// shift to positive numbers |
249 | stickThrottle = RCChannel(CH_THROTTLE) + 120;// shift to positive numbers |
250 | |
250 | |
251 | if(externalControl.config & 0x01 && dynamicParams.ExternalControl > 128) { |
251 | if(externalControl.config & 0x01 && dynamicParams.ExternalControl > 128) { |
252 | stickPitch += (int16_t) externalControl.pitch * (int16_t) staticParams.StickP; |
252 | stickPitch += (int16_t) externalControl.pitch * (int16_t) staticParams.StickP; |
253 | stickRoll += (int16_t) externalControl.roll * (int16_t) staticParams.StickP; |
253 | stickRoll += (int16_t) externalControl.roll * (int16_t) staticParams.StickP; |
254 | stickYaw += externalControl.yaw; |
254 | stickYaw += externalControl.yaw; |
255 | // ExternHeightValue = (int16_t) ExternControl.Height * (int16_t)staticParams.Height_Gain; |
255 | // ExternHeightValue = (int16_t) ExternControl.Height * (int16_t)staticParams.Height_Gain; |
256 | // Dubious: Lowest throttle setting has precedence. |
256 | // Dubious: Lowest throttle setting has precedence. |
257 | if(externalControl.throttle < stickThrottle) stickThrottle = externalControl.throttle; |
257 | if(externalControl.throttle < stickThrottle) stickThrottle = externalControl.throttle; |
258 | } |
258 | } |
259 | |
259 | |
260 | if(stickThrottle < 0) stickThrottle = 0; |
260 | if(stickThrottle < 0) stickThrottle = 0; |
261 | 261 | ||
262 | if(abs(stickPitch / STICK_GAIN) > maxStickPitch) { |
262 | if(abs(stickPitch / CONTROL_SCALING) > maxStickPitch) { |
263 | maxStickPitch = abs(stickPitch) / STICK_GAIN; |
263 | maxStickPitch = abs(stickPitch) / CONTROL_SCALING; |
264 | if(maxStickPitch > 100) maxStickPitch = 100; |
264 | if(maxStickPitch > 100) maxStickPitch = 100; |
265 | } |
265 | } |
266 | else if (maxStickPitch) maxStickPitch--; |
266 | else if (maxStickPitch) maxStickPitch--; |
267 | 267 | ||
268 | if(abs(stickRoll / STICK_GAIN) > maxStickRoll) { |
268 | if(abs(stickRoll / CONTROL_SCALING) > maxStickRoll) { |
269 | maxStickRoll = abs(stickRoll) / STICK_GAIN; |
269 | maxStickRoll = abs(stickRoll) / CONTROL_SCALING; |
270 | if(maxStickRoll > 100) maxStickRoll = 100; |
270 | if(maxStickRoll > 100) maxStickRoll = 100; |
271 | } |
271 | } |
272 | else if (maxStickRoll) maxStickRoll--; |
272 | else if (maxStickRoll) maxStickRoll--; |
273 | 273 | ||
274 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
274 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
275 | // Looping? Do not consider external or GPS input for this :) |
275 | // Looping? Do not consider external or GPS input for this :) |
276 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
276 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
277 | if((RCChannel(CH_ROLL) > staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_LEFT) loopingLeft = 1; |
277 | if((RCChannel(CH_ROLL) > staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_LEFT) loopingLeft = 1; |
278 | else { |
278 | else { |
279 | if(loopingLeft) { // Hysteresis |
279 | if(loopingLeft) { // Hysteresis |
280 | if((RCChannel(CH_ROLL) < (staticParams.LoopThreshold - staticParams.LoopHysteresis))) loopingLeft = 0; |
280 | if((RCChannel(CH_ROLL) < (staticParams.LoopThreshold - staticParams.LoopHysteresis))) loopingLeft = 0; |
281 | } |
281 | } |
282 | } |
282 | } |
283 | |
283 | |
284 | if((RCChannel(CH_ROLL) < -staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_RIGHT) loopingRight = 1; |
284 | if((RCChannel(CH_ROLL) < -staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_RIGHT) loopingRight = 1; |
285 | else { |
285 | else { |
286 | if(loopingRight) { // Hysteresis |
286 | if(loopingRight) { // Hysteresis |
287 | if(RCChannel(CH_ROLL) > -(staticParams.LoopThreshold - staticParams.LoopHysteresis)) loopingRight = 0; |
287 | if(RCChannel(CH_ROLL) > -(staticParams.LoopThreshold - staticParams.LoopHysteresis)) loopingRight = 0; |
288 | } |
288 | } |
289 | } |
289 | } |
290 | |
290 | |
291 | if((RCChannel(CH_PITCH) > staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_UP) loopingTop = 1; |
291 | if((RCChannel(CH_PITCH) > staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_UP) loopingTop = 1; |
292 | else { |
292 | else { |
293 | if(loopingTop) { // Hysteresis |
293 | if(loopingTop) { // Hysteresis |
294 | if((RCChannel(CH_PITCH) < (staticParams.LoopThreshold - staticParams.LoopHysteresis))) loopingTop = 0; |
294 | if((RCChannel(CH_PITCH) < (staticParams.LoopThreshold - staticParams.LoopHysteresis))) loopingTop = 0; |
295 | } |
295 | } |
296 | } |
296 | } |
297 | |
297 | |
298 | if((RCChannel(CH_PITCH) < -staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_DOWN) loopingDown = 1; |
298 | if((RCChannel(CH_PITCH) < -staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_DOWN) loopingDown = 1; |
299 | else { |
299 | else { |
300 | if(loopingDown) { // Hysteresis |
300 | if(loopingDown) { // Hysteresis |
301 | if(RCChannel(CH_PITCH) > -(staticParams.LoopThreshold - staticParams.LoopHysteresis)) loopingDown = 0; |
301 | if(RCChannel(CH_PITCH) > -(staticParams.LoopThreshold - staticParams.LoopHysteresis)) loopingDown = 0; |
302 | } |
302 | } |
303 | } |
303 | } |
304 | |
304 | |
305 | if(loopingLeft || loopingRight) loopingRoll = 1; else loopingRoll = 0; |
305 | if(loopingLeft || loopingRight) loopingRoll = 1; else loopingRoll = 0; |
306 | if(loopingTop || loopingDown) { loopingPitch = 1; loopingRoll = 0; loopingLeft = 0; loopingRight = 0;} else loopingPitch = 0; |
306 | if(loopingTop || loopingDown) { loopingPitch = 1; loopingRoll = 0; loopingLeft = 0; loopingRight = 0;} else loopingPitch = 0; |
307 | } |
307 | } |
308 | 308 | ||
309 | void setCompassCalState(void) { |
309 | void setCompassCalState(void) { |
310 | static uint8_t stick = 1; |
310 | static uint8_t stick = 1; |
311 | // if pitch is centered or top set stick to zero |
311 | // if pitch is centered or top set stick to zero |
312 | if(RCChannel(CH_PITCH) > -20) stick = 0; |
312 | if(RCChannel(CH_PITCH) > -20) stick = 0; |
313 | // if pitch is down trigger to next cal state |
313 | // if pitch is down trigger to next cal state |
314 | if((RCChannel(CH_PITCH) < -70) && !stick) { |
314 | if((RCChannel(CH_PITCH) < -70) && !stick) { |
315 | stick = 1; |
315 | stick = 1; |
316 | compassCalState++; |
316 | compassCalState++; |
317 | if(compassCalState < 5) beepNumber(compassCalState); |
317 | if(compassCalState < 5) beepNumber(compassCalState); |
318 | else beep(1000); |
318 | else beep(1000); |
319 | } |
319 | } |
320 | } |
320 | } |
321 | 321 | ||
322 | / * |
322 | / * |
323 | * |
323 | * |
324 | * / |
324 | * / |
325 | uint8_t control_hasNewRCData(void) { |
325 | uint8_t control_hasNewRCData(void) { |
326 | // return !NewPpmData--; |
326 | // return !NewPpmData--; |
327 | return (NewPpmData-- == 0) ? 1 : 0; |
327 | return (NewPpmData-- == 0) ? 1 : 0; |
328 | } |
328 | } |
329 | 329 | ||
330 | void control_performPilotCalibrationCommands(uint8_t stickCommand) { |
330 | void control_performPilotCalibrationCommands(uint8_t stickCommand) { |
331 | if (stickCommand == STICK_COMMAND_GYROCAL && !control_isStickCommandRepeated()) { |
331 | if (stickCommand == STICK_COMMAND_GYROCAL && !control_isStickCommandRepeated()) { |
332 | // Run gyro calibration but do not repeat it. |
332 | // Run gyro calibration but do not repeat it. |
333 | GRN_OFF; |
333 | GRN_OFF; |
334 | |
334 | |
335 | // TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough? |
335 | // TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough? |
336 | // isFlying = 0; |
336 | // isFlying = 0; |
337 | // check roll/pitch stick position |
337 | // check roll/pitch stick position |
338 | // if pitch stick is top or roll stick is left or right --> change parameter setting |
338 | // if pitch stick is top or roll stick is left or right --> change parameter setting |
339 | // according to roll/pitch stick position |
339 | // according to roll/pitch stick position |
340 | |
340 | |
341 | uint8_t setting = control_getRightRCStickIndex(); |
341 | uint8_t setting = control_getRightRCStickIndex(); |
342 | |
342 | |
343 | if ((setting > 0 && setting < 6) || setting == 9) { |
343 | if ((setting > 0 && setting < 6) || setting == 9) { |
344 | // Gyro calinbration, with or without selecting a new parameter-set. |
344 | // Gyro calinbration, with or without selecting a new parameter-set. |
345 | if(setting > 0 && setting < 6) { |
345 | if(setting > 0 && setting < 6) { |
346 | // A valid parameter-set (1..5) was chosen - use it. |
346 | // A valid parameter-set (1..5) was chosen - use it. |
347 | setActiveParamSet(setting); |
347 | setActiveParamSet(setting); |
348 | } |
348 | } |
349 | ParamSet_ReadFromEEProm(getActiveParamSet()); |
349 | ParamSet_ReadFromEEProm(getActiveParamSet()); |
350 | attitude_setNeutral(); |
350 | attitude_setNeutral(); |
351 | flight_setNeutral(); |
351 | flight_setNeutral(); |
352 | if (setting == 9) { // Right stick is centered; calibrate it to zero (hmm strictly does not belong here). |
352 | if (setting == 9) { // Right stick is centered; calibrate it to zero (hmm strictly does not belong here). |
353 | control_setNeutral(); // Calibrate right stick neutral position. |
353 | control_setNeutral(); // Calibrate right stick neutral position. |
354 | } |
354 | } |
355 | beepNumber(getActiveParamSet()); |
355 | beepNumber(getActiveParamSet()); |
356 | } else if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE) && setting == 7) { |
356 | } else if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE) && setting == 7) { |
357 | // If right stick is centered and down |
357 | // If right stick is centered and down |
358 | compassCalState = 1; |
358 | compassCalState = 1; |
359 | beep(1000); |
359 | beep(1000); |
360 | } |
360 | } |
361 | } |
361 | } |
362 | |
362 | |
363 | // save the ACC neutral setting to eeprom |
363 | // save the ACC neutral setting to eeprom |
364 | else { |
364 | else { |
365 | if(stickCommand == STICK_COMMAND_ACCCAL && !control_isStickCommandRepeated()) { |
365 | if(stickCommand == STICK_COMMAND_ACCCAL && !control_isStickCommandRepeated()) { |
366 | // Run gyro and acc. meter calibration but do not repeat it. |
366 | // Run gyro and acc. meter calibration but do not repeat it. |
367 | GRN_OFF; |
367 | GRN_OFF; |
368 | analog_calibrateAcc(); |
368 | analog_calibrateAcc(); |
369 | attitude_setNeutral(); |
369 | attitude_setNeutral(); |
370 | flight_setNeutral(); |
370 | flight_setNeutral(); |
371 | control_setNeutral(); // Calibrate right stick neutral position. |
371 | control_setNeutral(); // Calibrate right stick neutral position. |
372 | beepNumber(getActiveParamSet()); |
372 | beepNumber(getActiveParamSet()); |
373 | } |
373 | } |
374 | } |
374 | } |
375 | } // end !MOTOR_RUN condition. |
375 | } // end !MOTOR_RUN condition. |
376 | /* |
376 | /* |
377 | 377 |