Rev 1908 | Rev 1960 | 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), |
||
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) |
||
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 |
||
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 | |||
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" |
58 | #include "configuration.h" |
||
59 | #include "attitude.h" |
||
1775 | - | 60 | #include "commands.h" |
61 | #include "output.h" |
||
1612 | dongfang | 62 | |
1908 | - | 63 | // uint16_t maxControl[2] = { 0, 0 }; |
64 | uint16_t controlActivity = 0; |
||
65 | int16_t controls[4] = { 0, 0, 0, 0 }; |
||
66 | //int16_t controlYaw = 0, controlThrottle = 0; |
||
1612 | dongfang | 67 | uint8_t looping = 0; |
68 | |||
69 | // Internal variables for reading commands made with an R/C stick. |
||
1821 | - | 70 | uint8_t lastCommand = COMMAND_NONE; |
1775 | - | 71 | uint8_t lastArgument; |
1612 | dongfang | 72 | |
1775 | - | 73 | uint8_t isCommandRepeated = 0; |
74 | |||
75 | // MK flags. TODO: Replace by enum. State machine. |
||
76 | uint16_t isFlying = 0; |
||
77 | volatile uint8_t MKFlags = 0; |
||
78 | |||
1612 | dongfang | 79 | /* |
80 | * This could be expanded to take arguments from ohter sources than the RC |
||
81 | * (read: Custom MK RC project) |
||
82 | */ |
||
83 | uint8_t controlMixer_getArgument(void) { |
||
1821 | - | 84 | return lastArgument; |
1612 | dongfang | 85 | } |
86 | |||
87 | /* |
||
88 | * This could be expanded to take calibrate / start / stop commands from ohter sources |
||
89 | * than the R/C (read: Custom MK R/C project) |
||
90 | */ |
||
91 | uint8_t controlMixer_getCommand(void) { |
||
1821 | - | 92 | return lastCommand; |
1612 | dongfang | 93 | } |
94 | |||
95 | uint8_t controlMixer_isCommandRepeated(void) { |
||
1821 | - | 96 | return isCommandRepeated; |
1612 | dongfang | 97 | } |
98 | |||
1775 | - | 99 | void controlMixer_setNeutral() { |
1821 | - | 100 | EC_setNeutral(); |
101 | HC_setGround(); |
||
1612 | dongfang | 102 | } |
103 | |||
104 | /* |
||
105 | * Set the potientiometer values to the momentary values of the respective R/C channels. |
||
106 | * No slew rate limitation. |
||
107 | */ |
||
108 | void controlMixer_initVariables(void) { |
||
1821 | - | 109 | uint8_t i; |
110 | for (i = 0; i < 8; i++) { |
||
111 | variables[i] = RC_getVariable(i); |
||
112 | } |
||
1612 | dongfang | 113 | } |
114 | |||
115 | /* |
||
116 | * Update potentiometer values with limited slew rate. Could be made faster if desired. |
||
1775 | - | 117 | * TODO: It assumes R/C as source. Not necessarily true. |
1612 | dongfang | 118 | */ |
119 | void controlMixer_updateVariables(void) { |
||
1821 | - | 120 | uint8_t i; |
121 | int16_t targetvalue; |
||
122 | for (i = 0; i < 8; i++) { |
||
123 | targetvalue = RC_getVariable(i); |
||
124 | if (targetvalue < 0) |
||
125 | targetvalue = 0; |
||
126 | if (variables[i] < targetvalue && variables[i] < 255) |
||
127 | variables[i]++; |
||
128 | else if (variables[i] > 0 && variables[i] > targetvalue) |
||
129 | variables[i]--; |
||
130 | } |
||
1612 | dongfang | 131 | } |
132 | |||
133 | uint8_t controlMixer_getSignalQuality(void) { |
||
1821 | - | 134 | uint8_t rcQ = RC_getSignalQuality(); |
135 | uint8_t ecQ = EC_getSignalQuality(); |
||
136 | // This needs not be the only correct solution... |
||
137 | return rcQ > ecQ ? rcQ : ecQ; |
||
1612 | dongfang | 138 | } |
139 | |||
1908 | - | 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 | } |
||
159 | |||
1612 | dongfang | 160 | /* |
161 | * Update the variables indicating stick position from the sum of R/C, GPS and external control. |
||
162 | */ |
||
163 | void controlMixer_update(void) { |
||
1821 | - | 164 | // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
165 | // TODO: If no signal --> zero. |
||
1872 | - | 166 | int16_t tempThrottle; |
1775 | - | 167 | |
1821 | - | 168 | // takes almost no time... |
169 | RC_update(); |
||
1612 | dongfang | 170 | |
1821 | - | 171 | // takes almost no time... |
172 | EC_update(); |
||
1775 | - | 173 | |
1821 | - | 174 | // takes about 80 usec. |
175 | HC_update(); |
||
1612 | dongfang | 176 | |
1908 | - | 177 | int16_t* RC_PRTY = RC_getPRTY(); |
178 | int16_t* EC_PRTY = EC_getPRTY(); |
||
1634 | - | 179 | |
1908 | - | 180 | updateControlAndMeasureControlActivity(CONTROL_PITCH, RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]); |
181 | updateControlAndMeasureControlActivity(CONTROL_ROLL, RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]); |
||
182 | updateControlAndMeasureControlActivity(CONTROL_YAW, RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]); |
||
183 | dampenControlActivity(); |
||
1872 | - | 184 | |
1955 | - | 185 | //debugOut.analog[14] = controlActivity/10; |
1908 | - | 186 | |
1872 | - | 187 | tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]); |
1908 | - | 188 | controls[CONTROL_THROTTLE] = AC_getThrottle(tempThrottle); |
1872 | - | 189 | |
1908 | - | 190 | // controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
1775 | - | 191 | |
1821 | - | 192 | if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
193 | controlMixer_updateVariables(); |
||
194 | configuration_applyVariablesToParams(); |
||
195 | looping = RC_getLooping(looping); |
||
196 | } else { // Signal is not OK |
||
197 | // Could handle switch to emergency flight here. |
||
198 | // throttle is handled elsewhere. |
||
199 | looping = 0; |
||
200 | } |
||
1612 | dongfang | 201 | |
1821 | - | 202 | // part1a end. |
1612 | dongfang | 203 | |
1869 | - | 204 | /* This is not really necessary with the dead-band feature on all sticks (see rc.c) |
1821 | - | 205 | if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
206 | if (controlYaw > 2) controlYaw-= 2; |
||
207 | else if (controlYaw< -2) controlYaw += 2; |
||
208 | else controlYaw = 0; |
||
209 | } |
||
210 | */ |
||
1612 | dongfang | 211 | |
1821 | - | 212 | /* |
213 | * Record maxima |
||
214 | for (axis = PITCH; axis <= ROLL; axis++) { |
||
215 | if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) { |
||
216 | maxControl[axis] = abs(control[axis]) / CONTROL_SCALING; |
||
217 | if (maxControl[axis] > 100) |
||
218 | maxControl[axis] = 100; |
||
219 | } else if (maxControl[axis]) |
||
220 | maxControl[axis]--; |
||
221 | } |
||
1908 | - | 222 | */ |
1612 | dongfang | 223 | |
1821 | - | 224 | uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() |
225 | : COMMAND_NONE; |
||
226 | uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() |
||
227 | : COMMAND_NONE; |
||
1775 | - | 228 | |
1821 | - | 229 | if (rcCommand != COMMAND_NONE) { |
230 | isCommandRepeated = (lastCommand == rcCommand); |
||
231 | lastCommand = rcCommand; |
||
232 | lastArgument = RC_getArgument(); |
||
233 | } else if (ecCommand != COMMAND_NONE) { |
||
234 | isCommandRepeated = (lastCommand == ecCommand); |
||
235 | lastCommand = ecCommand; |
||
236 | lastArgument = EC_getArgument(); |
||
237 | } else { |
||
238 | // Both sources have no command, or one or both are out. |
||
239 | // Just set to false. There is no reason to check if the none-command was repeated anyway. |
||
240 | isCommandRepeated = 0; |
||
241 | lastCommand = COMMAND_NONE; |
||
242 | } |
||
243 | |||
1872 | - | 244 | /* |
1821 | - | 245 | if (isCommandRepeated) |
246 | DebugOut.Digital[0] |= DEBUG_COMMANDREPEATED; |
||
247 | else |
||
248 | DebugOut.Digital[0] &= ~DEBUG_COMMANDREPEATED; |
||
249 | if (rcCommand) |
||
250 | DebugOut.Digital[1] |= DEBUG_COMMANDREPEATED; |
||
251 | else |
||
252 | DebugOut.Digital[1] &= ~DEBUG_COMMANDREPEATED; |
||
1872 | - | 253 | */ |
1612 | dongfang | 254 | } |
255 | |||
1775 | - | 256 | // TODO: Integrate into command system. |
257 | uint8_t controlMixer_testCompassCalState(void) { |
||
1821 | - | 258 | return RC_testCompassCalState(); |
1612 | dongfang | 259 | } |