Subversion Repositories FlightCtrl

Rev

Details | 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
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1623 - 51
 
52
/*
53
OBSOLETED BY controlMixer.c. But this is how it looked - maybe somebody will find it simpler?
54
 
1612 dongfang 55
#include <stdlib.h>
56
#include "control.h"
57
 
58
#include "rc.h"
59
#include "configuration.h"
60
#include "attitude.h"
61
#include "eeprom.h"
62
#include "flight.h"
63
 
64
#define RCChannel(dimension) (PPM_in[staticParams.ChannelAssignment[dimension]])
65
 
66
uint16_t maxStickPitch = 0, maxStickRoll = 0;
67
int16_t stickPitch = 0, stickRoll = 0, stickYaw = 0, stickThrottle = 0;
68
int16_t GPSStickPitch = 0, GPSStickRoll = 0;
69
int16_t externalStickPitch = 0, externalStickRoll = 0, externalStickYaw = 0, externalHeightValue = -20;
70
 
71
// dongfang's own experiment: Cablibrated sticks.
72
int16_t stickOffsetPitch = 0, stickOffsetRoll = 0;
73
 
74
// Looping-or-not flags.
75
uint8_t loopingPitch = 0, loopingRoll = 0;
76
uint8_t loopingLeft = 0, loopingRight = 0, loopingDown = 0, loopingTop = 0;
77
 
78
// Internal variables for reading commands made with an R/S stick.
79
uint8_t lastStickCommand  = STICK_COMMAND_UNDEF;
80
uint8_t stickCommandTimer = 0;
81
 
82
ExternalControl_t externalControl;
83
 
1623 - 84
/ *
1612 dongfang 85
 * Stick diagram:
86
 * 2--3--4
87
 * |     |  +
88
 * 1  9  5  ^ 0
89
 * |     |  |  
90
 * 8--7--6
91
 *    
92
 * + <--
93
 *    0
94
 *
95
 * Not in any of these positions: 0
1623 - 96
 * /
1612 dongfang 97
 
1623 - 98
/ *
1612 dongfang 99
 * The stick most be further from center than this to indicate a settings number (1-5).
1623 - 100
 * /
1612 dongfang 101
#define STICK_SETTINGSELECTION_THRESHOLD 70
102
 
103
uint8_t control_getLeftRCStickIndex(int16_t thresholdThrottle, int16_t thresholdYaw) {
104
  if(RCChannel(CH_THROTTLE) > thresholdThrottle) {
105
    // throttle is up
106
    if(RCChannel(CH_YAW) > thresholdYaw)
107
      return STICK_COMMAND_GYROCAL;
108
    if(RCChannel(CH_YAW) < -thresholdYaw)
109
      return STICK_COMMAND_ACCCAL;
110
    return STICK_COMMAND_UNDEF;
111
  } else if(RCChannel(CH_THROTTLE) < -thresholdThrottle) {
112
    // pitch is down
113
    if(RCChannel(CH_YAW) > thresholdYaw)
114
      return STICK_COMMAND_STOP;
115
    if(RCChannel(CH_YAW) < -thresholdYaw)
116
      return STICK_COMMAND_START;
117
    return STICK_COMMAND_UNDEF;
118
  } else {
119
    // pitch is around center
120
    return STICK_COMMAND_UNDEF;
121
  }
122
}
123
 
124
uint8_t control_getRightRCStickIndex(void) {
125
  if(RCChannel(CH_PITCH) > STICK_SETTINGSELECTION_THRESHOLD) {
126
    // pitch is up
127
    if(RCChannel(CH_ROLL) > STICK_SETTINGSELECTION_THRESHOLD)
128
      return 2;
129
    if(RCChannel(CH_ROLL) < -STICK_SETTINGSELECTION_THRESHOLD)
130
      return 4;
131
    return 3;
132
  } else if(RCChannel(CH_PITCH) < -STICK_SETTINGSELECTION_THRESHOLD) {
133
    // pitch is down
134
    if(RCChannel(CH_ROLL) > STICK_SETTINGSELECTION_THRESHOLD)
135
      return 8;
136
    if(RCChannel(CH_ROLL) < -STICK_SETTINGSELECTION_THRESHOLD)
137
      return 6;
138
    return 7;
139
  } else {
140
    // pitch is around center
141
    if(RCChannel(CH_ROLL) > STICK_SETTINGSELECTION_THRESHOLD)
142
      return 1;
143
    if(RCChannel(CH_ROLL) < -STICK_SETTINGSELECTION_THRESHOLD)
144
      return 5;
145
    return 9;
146
  }
147
}
148
 
1623 - 149
/ *
1612 dongfang 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)
1623 - 152
 * /
1612 dongfang 153
void control_senseStickCommands(void) {
154
  uint8_t stickCommandNow = control_getLeftRCStickIndex(85, 85);
155
  if (stickCommandNow != lastStickCommand) {
156
    lastStickCommand = stickCommandNow;
157
    stickCommandTimer = 0;
158
  } else {
159
    if (stickCommandTimer < 201)
160
      stickCommandTimer++;
161
  }
162
}
163
 
1623 - 164
/ *
1612 dongfang 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)
1623 - 167
 * /
1612 dongfang 168
uint8_t control_getStickCommand(void) {
169
  // If the same command was made 200 times, it's stable.
170
  if (stickCommandTimer >= 200) {
171
    return lastStickCommand;
172
  }
173
  return STICK_COMMAND_UNDEF;
174
}
175
 
176
uint8_t control_isStickCommandRepeated(void) {
177
  return stickCommandTimer > 200 ? 1 : 0;
178
}
179
 
1623 - 180
/ *
1612 dongfang 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
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
185
 * R617 receiver.) This calibration is not strictly necessary, but
186
 * for control logic that depends on the exact (non)center position
187
 * of a stick, it may be useful.
1623 - 188
 * /
1612 dongfang 189
void control_setNeutral(void) {
190
  stickOffsetPitch += stickPitch;
191
  stickOffsetRoll += stickRoll;
192
}
193
 
1623 - 194
/ *
1612 dongfang 195
 * Set the potientiometer values to the values of the respective R/C channel
196
 * right now. No slew rate limit.
1623 - 197
 * /
1612 dongfang 198
void control_initPots(void) {
199
  uint8_t i;
200
  for (i=0; i<4; i++) {
201
    pots[i] = RCChannel(CH_POTS + i) + POT_OFFSET;
202
  }
203
  for (i=4; i<8; i++) {
204
    pots[i] = PPM_in[9 + (i-4)] + POT_OFFSET;
205
  }
206
}
207
 
1623 - 208
/ *
1612 dongfang 209
 * Update potentiometer values with slow slew rate. Could be made faster if desired.
1623 - 210
 * /
1612 dongfang 211
void control_updatePots(void) {
212
  uint8_t i;
213
  uint16_t targetvalue;
214
  for (i=0; i<8; i++) {
215
    if (i<4)  // configured pots
216
      targetvalue = RCChannel(CH_POTS + i) + POT_OFFSET;
217
    else      // PPM24-Extension
218
      targetvalue = PPM_in[9 + i] + POT_OFFSET;
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]--;
221
  }
222
}
223
 
1623 - 224
/ *
1612 dongfang 225
 * Update the variables indicating stick position from the sum of R/C, GPS and external control.
1623 - 226
 * /
1612 dongfang 227
void control_update(void) {
228
  // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
229
  stickPitch = RCChannel(CH_PITCH) * staticParams.StickP;
230
  // (stick_pitch * 3 + RCChannel(CH_PITCH) * staticParams.StickP) / 4;
231
  stickPitch += PPM_diff[staticParams.ChannelAssignment[CH_PITCH]] * staticParams.StickD;
232
  stickPitch = stickPitch - stickOffsetPitch - GPSStickPitch;
233
 
234
  stickRoll = RCChannel(CH_ROLL) * staticParams.StickP;
235
  // stick_roll = (stick_roll * 3 + RCChannel(CH_ROLL) * staticParams.StickP) / 4;
236
  stickRoll += PPM_diff[staticParams.ChannelAssignment[CH_ROLL]] * staticParams.StickD;
237
  stickRoll = stickRoll - stickOffsetRoll - GPSStickRoll;
238
 
239
  // mapping of yaw
240
  stickYaw = -RCChannel(CH_YAW);
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)) {
243
    if (stickYaw > 2) stickYaw-= 2;
244
    else if (stickYaw< -2) stickYaw += 2;
245
    else stickYaw = 0;
246
  }
247
 
248
  // mapping of gas
249
  stickThrottle = RCChannel(CH_THROTTLE) + 120;// shift to positive numbers
250
 
251
  if(externalControl.config & 0x01 && dynamicParams.ExternalControl > 128) {
252
    stickPitch += (int16_t) externalControl.pitch * (int16_t) staticParams.StickP;
253
    stickRoll += (int16_t) externalControl.roll * (int16_t) staticParams.StickP;
254
    stickYaw += externalControl.yaw;
255
    // ExternHeightValue =  (int16_t) ExternControl.Height * (int16_t)staticParams.Height_Gain;
256
    // Dubious: Lowest throttle setting has precedence.
257
    if(externalControl.throttle < stickThrottle) stickThrottle = externalControl.throttle;
258
  }
259
 
260
  if(stickThrottle < 0) stickThrottle = 0;
261
 
1645 - 262
  if(abs(stickPitch / CONTROL_SCALING) > maxStickPitch) {
263
    maxStickPitch = abs(stickPitch) / CONTROL_SCALING;
1612 dongfang 264
    if(maxStickPitch > 100) maxStickPitch = 100;
265
  }
266
  else if (maxStickPitch) maxStickPitch--;
267
 
1645 - 268
  if(abs(stickRoll / CONTROL_SCALING) > maxStickRoll) {
269
    maxStickRoll = abs(stickRoll) / CONTROL_SCALING;
1612 dongfang 270
    if(maxStickRoll > 100) maxStickRoll = 100;
271
  }
272
  else if (maxStickRoll) maxStickRoll--;
273
 
274
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
275
  // Looping? Do not consider external or GPS input for this :)
276
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
277
  if((RCChannel(CH_ROLL) > staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_LEFT)  loopingLeft = 1;
278
  else {
279
    if(loopingLeft) { // Hysteresis
280
      if((RCChannel(CH_ROLL) < (staticParams.LoopThreshold - staticParams.LoopHysteresis))) loopingLeft = 0;
281
    }
282
  }
283
 
284
  if((RCChannel(CH_ROLL) < -staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_RIGHT) loopingRight = 1;
285
  else {
286
    if(loopingRight) { // Hysteresis
287
      if(RCChannel(CH_ROLL) > -(staticParams.LoopThreshold - staticParams.LoopHysteresis)) loopingRight = 0;
288
    }
289
  }
290
 
291
  if((RCChannel(CH_PITCH) > staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_UP) loopingTop = 1;
292
  else {
293
    if(loopingTop) { // Hysteresis
294
      if((RCChannel(CH_PITCH) < (staticParams.LoopThreshold - staticParams.LoopHysteresis))) loopingTop = 0;
295
    }
296
  }
297
 
298
  if((RCChannel(CH_PITCH) < -staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_DOWN) loopingDown = 1;
299
  else {
300
    if(loopingDown) { // Hysteresis
301
      if(RCChannel(CH_PITCH) > -(staticParams.LoopThreshold - staticParams.LoopHysteresis)) loopingDown = 0;
302
    }
303
  }
304
 
305
  if(loopingLeft || loopingRight)  loopingRoll = 1; else loopingRoll = 0;
306
  if(loopingTop  || loopingDown) { loopingPitch = 1; loopingRoll = 0; loopingLeft = 0; loopingRight = 0;} else  loopingPitch = 0;
307
}
308
 
309
void setCompassCalState(void) {
310
  static uint8_t stick = 1;
311
  // if pitch is centered or top set stick to zero
312
  if(RCChannel(CH_PITCH) > -20) stick = 0;
313
  // if pitch is down trigger to next cal state
314
  if((RCChannel(CH_PITCH) < -70) && !stick) {
315
    stick = 1;
316
    compassCalState++;
317
    if(compassCalState < 5) beepNumber(compassCalState);
318
    else beep(1000);
319
  }
320
}
321
 
1623 - 322
/ *
1612 dongfang 323
 *
1623 - 324
 * /
1612 dongfang 325
uint8_t control_hasNewRCData(void) {
326
  // return !NewPpmData--;
327
  return (NewPpmData-- == 0) ? 1 : 0;
328
}
329
 
330
void control_performPilotCalibrationCommands(uint8_t stickCommand) {
331
  if (stickCommand == STICK_COMMAND_GYROCAL && !control_isStickCommandRepeated()) {
332
    // Run gyro calibration but do not repeat it.
333
    GRN_OFF;
334
 
335
    // TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough?
336
    // isFlying = 0;
337
    // check roll/pitch stick position
338
    // if pitch stick is top or roll stick is left or right --> change parameter setting
339
    // according to roll/pitch stick position
340
 
341
    uint8_t setting = control_getRightRCStickIndex();
342
 
343
    if ((setting > 0 && setting < 6) || setting == 9) {
344
      // Gyro calinbration, with or without selecting a new parameter-set.
345
      if(setting > 0 && setting < 6) {
346
        // A valid parameter-set (1..5) was chosen - use it.
347
        setActiveParamSet(setting);
348
      }
349
      ParamSet_ReadFromEEProm(getActiveParamSet());
350
      attitude_setNeutral();
351
      flight_setNeutral();
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.
354
      }
355
      beepNumber(getActiveParamSet());
356
    } else if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE) && setting == 7) {
357
      // If right stick is centered and down
358
      compassCalState = 1;
359
      beep(1000);
360
    }
361
  }
362
 
363
  // save the ACC neutral setting to eeprom
364
  else  {
365
    if(stickCommand == STICK_COMMAND_ACCCAL && !control_isStickCommandRepeated()) {
366
      // Run gyro and acc. meter calibration but do not repeat it.
367
      GRN_OFF;
368
      analog_calibrateAcc();
369
      attitude_setNeutral();
370
      flight_setNeutral();
371
      control_setNeutral(); // Calibrate right stick neutral position.
372
      beepNumber(getActiveParamSet());
373
    }
374
  }
375
} // end !MOTOR_RUN condition.
1623 - 376
/*