Subversion Repositories FlightCtrl

Rev

Rev 1796 | Rev 1867 | 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)
35
// +     Commercial use (for excample: 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
 
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
 
1821 - 63
uint16_t maxControl[2] = { 0, 0 };
64
int16_t control[2] = { 0, 0 };
1775 - 65
int16_t controlYaw = 0, controlThrottle = 0;
1612 dongfang 66
uint8_t looping = 0;
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;
73
 
74
// MK flags. TODO: Replace by enum. State machine.
75
uint16_t isFlying = 0;
76
volatile uint8_t MKFlags = 0;
77
 
1612 dongfang 78
/*
79
 * This could be expanded to take arguments from ohter sources than the RC
80
 * (read: Custom MK RC project)
81
 */
82
uint8_t controlMixer_getArgument(void) {
1821 - 83
        return lastArgument;
1612 dongfang 84
}
85
 
86
/*
87
 * This could be expanded to take calibrate / start / stop commands from ohter sources
88
 * than the R/C (read: Custom MK R/C project)
89
 */
90
uint8_t controlMixer_getCommand(void) {
1821 - 91
        return lastCommand;
1612 dongfang 92
}
93
 
94
uint8_t controlMixer_isCommandRepeated(void) {
1821 - 95
        return isCommandRepeated;
1612 dongfang 96
}
97
 
1775 - 98
void controlMixer_setNeutral() {
1821 - 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) {
1821 - 108
        uint8_t i;
109
        for (i = 0; i < 8; 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) {
1821 - 119
        uint8_t i;
120
        int16_t targetvalue;
121
        for (i = 0; i < 8; 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) {
1821 - 133
        uint8_t rcQ = RC_getSignalQuality();
134
        uint8_t ecQ = EC_getSignalQuality();
135
        // This needs not be the only correct solution...
136
        return rcQ > ecQ ? rcQ : ecQ;
1612 dongfang 137
}
138
 
139
/*
140
 * Update the variables indicating stick position from the sum of R/C, GPS and external control.
141
 */
142
void controlMixer_update(void) {
1821 - 143
        // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
144
        // TODO: If no signal --> zero.
145
        uint8_t axis;
1775 - 146
 
1821 - 147
        // takes almost no time...
148
        RC_update();
1612 dongfang 149
 
1821 - 150
        // takes almost no time...
151
        EC_update();
1775 - 152
 
1821 - 153
        // takes about 80 usec.
154
        HC_update();
1612 dongfang 155
 
1821 - 156
        int16_t* RC_PRTY = RC_getPRTY();
157
        int16_t* EC_PRTY = EC_getPRTY();
1634 - 158
 
1821 - 159
        control[PITCH] = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH];
160
        control[ROLL] = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL];
161
        // This can be a CPU time killer if the function implementations are inefficient.
162
        controlThrottle = HC_getThrottle(AC_getThrottle(RC_PRTY[CONTROL_THROTTLE]
163
                        + EC_PRTY[CONTROL_THROTTLE]));
164
        controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];
1775 - 165
 
1821 - 166
        DebugOut.Analog[12] = control[PITCH];
167
        DebugOut.Analog[13] = control[ROLL];
168
        //DebugOut.Analog[26] = controlYaw;
1612 dongfang 169
 
1821 - 170
        if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
171
                controlMixer_updateVariables();
172
                configuration_applyVariablesToParams();
173
                looping = RC_getLooping(looping);
174
        } else { // Signal is not OK
175
                // Could handle switch to emergency flight here.
176
                // throttle is handled elsewhere.
177
                looping = 0;
178
        }
1612 dongfang 179
 
1821 - 180
        // part1a end.
1612 dongfang 181
 
1821 - 182
        /* This is not really necessary with the dead-gap feature on all sticks (see rc.c)
183
         if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
184
         if (controlYaw > 2) controlYaw-= 2;
185
         else if (controlYaw< -2) controlYaw += 2;
186
         else controlYaw = 0;
187
         }
188
         */
1612 dongfang 189
 
1821 - 190
        /*
191
         * Record maxima
192
         */
193
        for (axis = PITCH; axis <= ROLL; axis++) {
194
                if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) {
195
                        maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;
196
                        if (maxControl[axis] > 100)
197
                                maxControl[axis] = 100;
198
                } else if (maxControl[axis])
199
                        maxControl[axis]--;
200
        }
1612 dongfang 201
 
1821 - 202
        uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()
203
                        : COMMAND_NONE;
204
        uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()
205
                        : COMMAND_NONE;
1775 - 206
 
1821 - 207
        if (rcCommand != COMMAND_NONE) {
208
                isCommandRepeated = (lastCommand == rcCommand);
209
                lastCommand = rcCommand;
210
                lastArgument = RC_getArgument();
211
        } else if (ecCommand != COMMAND_NONE) {
212
                isCommandRepeated = (lastCommand == ecCommand);
213
                lastCommand = ecCommand;
214
                lastArgument = EC_getArgument();
215
        } else {
216
                // Both sources have no command, or one or both are out.
217
                // Just set to false. There is no reason to check if the none-command was repeated anyway.
218
                isCommandRepeated = 0;
219
                lastCommand = COMMAND_NONE;
220
        }
221
 
222
        if (isCommandRepeated)
223
                DebugOut.Digital[0] |= DEBUG_COMMANDREPEATED;
224
        else
225
                DebugOut.Digital[0] &= ~DEBUG_COMMANDREPEATED;
226
        if (rcCommand)
227
                DebugOut.Digital[1] |= DEBUG_COMMANDREPEATED;
228
        else
229
                DebugOut.Digital[1] &= ~DEBUG_COMMANDREPEATED;
230
 
231
        // part1 end.
1612 dongfang 232
}
233
 
1775 - 234
// TODO: Integrate into command system.
235
uint8_t controlMixer_testCompassCalState(void) {
1821 - 236
        return RC_testCompassCalState();
1612 dongfang 237
}