Subversion Repositories FlightCtrl

Rev

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
}