Rev 2039 | Rev 2048 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1612 | dongfang | 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), |
||
1963 | - | 7 | // + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zulässig ist. |
1612 | dongfang | 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 |
||
1963 | - | 17 | // + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
18 | // + eindeutig als Ursprung verlinkt und genannt werden |
||
1612 | dongfang | 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) |
||
1868 | - | 35 | // + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
1612 | dongfang | 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 |
||
1963 | - | 47 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
48 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
||
1612 | dongfang | 49 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
50 | // + POSSIBILITY OF SUCH DAMAGE. |
||
51 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
52 | #include <stdlib.h> |
||
53 | #include "controlMixer.h" |
||
54 | #include "rc.h" |
||
1775 | - | 55 | #include "heightControl.h" |
56 | #include "attitudeControl.h" |
||
1612 | dongfang | 57 | #include "externalControl.h" |
2039 | - | 58 | #include "naviControl.h" |
1612 | dongfang | 59 | #include "configuration.h" |
60 | #include "attitude.h" |
||
1775 | - | 61 | #include "commands.h" |
62 | #include "output.h" |
||
1612 | dongfang | 63 | |
1908 | - | 64 | // uint16_t maxControl[2] = { 0, 0 }; |
65 | uint16_t controlActivity = 0; |
||
66 | int16_t controls[4] = { 0, 0, 0, 0 }; |
||
1612 | dongfang | 67 | |
68 | // Internal variables for reading commands made with an R/C stick. |
||
1821 | - | 69 | uint8_t lastCommand = COMMAND_NONE; |
1775 | - | 70 | uint8_t lastArgument; |
1612 | dongfang | 71 | |
1775 | - | 72 | uint8_t isCommandRepeated = 0; |
1963 | - | 73 | uint8_t controlMixer_didReceiveSignal = 0; |
1775 | - | 74 | |
1612 | dongfang | 75 | /* |
76 | * This could be expanded to take arguments from ohter sources than the RC |
||
77 | * (read: Custom MK RC project) |
||
78 | */ |
||
79 | uint8_t controlMixer_getArgument(void) { |
||
1961 | - | 80 | return lastArgument; |
1612 | dongfang | 81 | } |
82 | |||
83 | /* |
||
84 | * This could be expanded to take calibrate / start / stop commands from ohter sources |
||
85 | * than the R/C (read: Custom MK R/C project) |
||
86 | */ |
||
87 | uint8_t controlMixer_getCommand(void) { |
||
1961 | - | 88 | return lastCommand; |
1612 | dongfang | 89 | } |
90 | |||
91 | uint8_t controlMixer_isCommandRepeated(void) { |
||
1961 | - | 92 | return isCommandRepeated; |
1612 | dongfang | 93 | } |
94 | |||
1775 | - | 95 | void controlMixer_setNeutral() { |
1961 | - | 96 | for (uint8_t i=0; i<VARIABLE_COUNT; i++) { |
97 | variables[i] = 0; |
||
98 | } |
||
99 | EC_setNeutral(); |
||
100 | HC_setGround(); |
||
1612 | dongfang | 101 | } |
102 | |||
103 | /* |
||
104 | * Set the potientiometer values to the momentary values of the respective R/C channels. |
||
105 | * No slew rate limitation. |
||
106 | */ |
||
107 | void controlMixer_initVariables(void) { |
||
1961 | - | 108 | uint8_t i; |
109 | for (i=0; i < VARIABLE_COUNT; i++) { |
||
110 | variables[i] = RC_getVariable(i); |
||
111 | } |
||
1612 | dongfang | 112 | } |
113 | |||
114 | /* |
||
115 | * Update potentiometer values with limited slew rate. Could be made faster if desired. |
||
1775 | - | 116 | * TODO: It assumes R/C as source. Not necessarily true. |
1612 | dongfang | 117 | */ |
118 | void controlMixer_updateVariables(void) { |
||
1961 | - | 119 | uint8_t i; |
120 | int16_t targetvalue; |
||
121 | for (i=0; i < VARIABLE_COUNT; i++) { |
||
122 | targetvalue = RC_getVariable(i); |
||
123 | if (targetvalue < 0) |
||
124 | targetvalue = 0; |
||
125 | if (variables[i] < targetvalue && variables[i] < 255) |
||
126 | variables[i]++; |
||
127 | else if (variables[i] > 0 && variables[i] > targetvalue) |
||
128 | variables[i]--; |
||
129 | } |
||
1612 | dongfang | 130 | } |
131 | |||
132 | uint8_t controlMixer_getSignalQuality(void) { |
||
1961 | - | 133 | uint8_t rcQ = RC_getSignalQuality(); |
134 | uint8_t ecQ = EC_getSignalQuality(); |
||
1964 | - | 135 | |
2018 | - | 136 | if (rcQ < SIGNAL_GOOD) debugOut.digital[0] |= DEBUG_SIGNAL; else debugOut.digital[0] &= ~DEBUG_SIGNAL; |
137 | if (ecQ < SIGNAL_GOOD) debugOut.digital[1] |= DEBUG_SIGNAL; else debugOut.digital[1] &= ~DEBUG_SIGNAL; |
||
1964 | - | 138 | |
1961 | - | 139 | // This needs not be the only correct solution... |
140 | return rcQ > ecQ ? rcQ : ecQ; |
||
1612 | dongfang | 141 | } |
142 | |||
1908 | - | 143 | void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) { |
1961 | - | 144 | int16_t tmp = controls[index]; |
145 | controls[index] = newValue; |
||
2017 | - | 146 | |
1961 | - | 147 | tmp -= newValue; |
148 | tmp /= 2; |
||
149 | tmp = tmp * tmp; |
||
150 | // tmp += (newValue >= 0) ? newValue : -newValue; |
||
1986 | - | 151 | /* |
1961 | - | 152 | if (controlActivity + (uint16_t)tmp >= controlActivity) |
153 | controlActivity += tmp; |
||
154 | else controlActivity = 0xffff; |
||
1986 | - | 155 | */ |
156 | if (controlActivity + (uint16_t)tmp < 32768) |
||
157 | controlActivity += tmp; |
||
1908 | - | 158 | } |
159 | |||
160 | #define CADAMPING 10 |
||
161 | void dampenControlActivity(void) { |
||
1961 | - | 162 | uint32_t tmp = controlActivity; |
163 | tmp *= ((1<<CADAMPING)-1); |
||
164 | tmp >>= CADAMPING; |
||
165 | controlActivity = tmp; |
||
1908 | - | 166 | } |
167 | |||
1612 | dongfang | 168 | /* |
169 | * Update the variables indicating stick position from the sum of R/C, GPS and external control. |
||
170 | */ |
||
2039 | - | 171 | void controlMixer_periodicTask(void) { |
1961 | - | 172 | // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
173 | // TODO: If no signal --> zero. |
||
174 | int16_t tempThrottle; |
||
175 | |||
176 | // takes almost no time... |
||
2039 | - | 177 | RC_periodicTask(); |
1961 | - | 178 | |
179 | // takes almost no time... |
||
2039 | - | 180 | EC_periodicTask(); |
1961 | - | 181 | |
182 | // takes about 80 usec. |
||
2039 | - | 183 | HC_periodicTask(); |
1961 | - | 184 | |
185 | int16_t* RC_PRTY = RC_getPRTY(); |
||
186 | int16_t* EC_PRTY = EC_getPRTY(); |
||
187 | |||
2045 | - | 188 | debugOut.analog[29] = RC_PRTY[CONTROL_PITCH]; |
189 | debugOut.analog[30] = RC_PRTY[CONTROL_ROLL]; |
||
190 | |||
2039 | - | 191 | int16_t naviSticks[] = {RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH], RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]}; |
192 | |||
193 | navigation_periodicTask(&naviSticks); |
||
194 | |||
195 | updateControlAndMeasureControlActivity(CONTROL_PITCH, naviSticks[CONTROL_PITCH]); |
||
196 | updateControlAndMeasureControlActivity(CONTROL_ROLL, naviSticks[CONTROL_ROLL]); |
||
1961 | - | 197 | updateControlAndMeasureControlActivity(CONTROL_YAW, RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]); |
198 | dampenControlActivity(); |
||
1775 | - | 199 | |
1986 | - | 200 | debugOut.analog[17] = controlActivity/10; |
1961 | - | 201 | |
202 | tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]); |
||
203 | controls[CONTROL_THROTTLE] = AC_getThrottle(tempThrottle); |
||
204 | |||
205 | if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
||
206 | controlMixer_updateVariables(); |
||
1963 | - | 207 | controlMixer_didReceiveSignal = 1; |
1961 | - | 208 | } else { // Signal is not OK |
209 | // Could handle switch to emergency flight here. |
||
210 | // throttle is handled elsewhere. |
||
211 | } |
||
1980 | - | 212 | |
213 | // We can safely do this even with a bad signal - the variables will not have been updated then. |
||
214 | configuration_applyVariablesToParams(); |
||
1961 | - | 215 | |
216 | // part1a end. |
||
217 | |||
218 | /* This is not really necessary with the dead-band feature on all sticks (see rc.c) |
||
219 | if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
||
220 | if (controlYaw > 2) controlYaw-= 2; |
||
221 | else if (controlYaw< -2) controlYaw += 2; |
||
222 | else controlYaw = 0; |
||
223 | } |
||
224 | */ |
||
225 | |||
226 | /* |
||
227 | * Record maxima |
||
228 | for (axis = PITCH; axis <= ROLL; axis++) { |
||
229 | if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) { |
||
230 | maxControl[axis] = abs(control[axis]) / CONTROL_SCALING; |
||
231 | if (maxControl[axis] > 100) |
||
232 | maxControl[axis] = 100; |
||
233 | } else if (maxControl[axis]) |
||
234 | maxControl[axis]--; |
||
235 | } |
||
236 | */ |
||
1612 | dongfang | 237 | |
1961 | - | 238 | uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() |
239 | : COMMAND_NONE; |
||
240 | uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() |
||
241 | : COMMAND_NONE; |
||
242 | |||
243 | if (rcCommand != COMMAND_NONE) { |
||
244 | isCommandRepeated = (lastCommand == rcCommand); |
||
245 | lastCommand = rcCommand; |
||
246 | lastArgument = RC_getArgument(); |
||
247 | } else if (ecCommand != COMMAND_NONE) { |
||
248 | isCommandRepeated = (lastCommand == ecCommand); |
||
249 | lastCommand = ecCommand; |
||
250 | lastArgument = EC_getArgument(); |
||
251 | } else { |
||
252 | // Both sources have no command, or one or both are out. |
||
253 | // Just set to false. There is no reason to check if the none-command was repeated anyway. |
||
254 | isCommandRepeated = 0; |
||
255 | lastCommand = COMMAND_NONE; |
||
256 | } |
||
1968 | - | 257 | |
2018 | - | 258 | //if (rcCommand != COMMAND_NONE) debugOut.digital[0] |= DEBUG_SIGNAL; else debugOut.digital[0] &= ~DEBUG_SIGNAL; |
259 | //if (isCommandRepeated) debugOut.digital[1] |= DEBUG_SIGNAL; else debugOut.digital[1] &= ~DEBUG_SIGNAL; |
||
1964 | - | 260 | } |
1612 | dongfang | 261 | |
1775 | - | 262 | // TODO: Integrate into command system. |
263 | uint8_t controlMixer_testCompassCalState(void) { |
||
1961 | - | 264 | return RC_testCompassCalState(); |
1612 | dongfang | 265 | } |