Subversion Repositories FlightCtrl

Rev

Rev 1645 | Rev 1796 | 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
1623 - 3
// + Nur für den privaten Gebrauch
1612 dongfang 4
// + www.MikroKopter.com
5
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1623 - 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 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.
1623 - 10
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
1612 dongfang 11
// + Verkauf von Luftbildaufnahmen, usw.
12
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1623 - 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
1612 dongfang 15
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
1623 - 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
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1623 - 20
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
1612 dongfang 21
// + Benutzung auf eigene Gefahr
1623 - 22
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
1612 dongfang 23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
1623 - 25
// + mit unserer Zustimmung zulässig
1612 dongfang 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
1623 - 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
 
53
#include <stdlib.h>
54
#include <avr/io.h>
55
#include "eeprom.h"
56
#include "flight.h"
57
 
58
// Only for debug. Remove.
1645 - 59
//#include "analog.h"
60
//#include "rc.h"
1612 dongfang 61
 
62
// Necessary for external control and motor test
63
#include "uart0.h"
64
#include "twimaster.h"
65
#include "attitude.h"
66
#include "controlMixer.h"
67
#ifdef USE_MK3MAG
68
#include "gps.h"
69
#endif
70
 
71
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
72
 
73
/*
74
 * These are no longer maintained, just left at 0. The original implementation just summed the acc.
75
 * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???
76
 */
1645 - 77
// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
1612 dongfang 78
 
79
// MK flags
80
uint16_t isFlying = 0;
81
volatile uint8_t MKFlags = 0;
82
 
83
uint8_t gyroPFactor, gyroIFactor;       // the PD factors for the attitude control
84
uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control
85
 
86
// Some integral weight constant...
87
uint16_t Ki = 10300 / 33;
88
uint8_t RequiredMotors = 0;
89
 
90
// No support for altitude control right now.
91
// int16_t SetPointHeight = 0;
92
 
93
/************************************************************************/
94
/*  Filter for motor value smoothing (necessary???)                     */
95
/************************************************************************/
96
int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
97
  switch(dynamicParams.UserParams[5]) {
98
  case 0:
99
    return newvalue;
100
  case 1:
101
    return (oldvalue + newvalue) / 2;  
102
  case 2:
103
    if(newvalue > oldvalue)
104
      return (1 * (int16_t)oldvalue + newvalue) / 2;  //mean of old and new
105
    else       
106
      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
107
  case 3:
108
    if(newvalue < oldvalue)
109
      return (1 * (int16_t)oldvalue + newvalue) / 2;  //mean of old and new
110
    else       
111
      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
112
  default: return newvalue;
113
  }
114
}
115
 
116
/************************************************************************/
117
/*  Neutral Readings                                                    */
118
/************************************************************************/
119
void flight_setNeutral() {
120
  // GPSStickPitch = 0;
121
  // GPSStickRoll = 0;
122
 
123
  MKFlags |= MKFLAG_CALIBRATE;
124
 
125
  // not really used here any more.
126
  dynamicParams.KalmanK = -1;
127
  dynamicParams.KalmanMaxDrift = 0;
128
  dynamicParams.KalmanMaxFusion = 32;
129
 
130
  controlMixer_initVariables();
131
}
132
 
133
/************************************************************************/
134
/*  Transmit Motor Data via I2C                                         */
135
/************************************************************************/
136
void sendMotorData(void) {
137
  uint8_t i;
138
  if(!(MKFlags & MKFLAG_MOTOR_RUN)) {
139
    // If pilot has not started the engines....
140
    MKFlags &= ~(MKFLAG_FLY | MKFLAG_START); // clear flag FLY and START if motors are off
141
    for(i = 0; i < MAX_MOTORS; i++) {
142
      // and if we are not in motor test mode, cut throttle on all motors.
143
      if(!motorTestActive) Motor[i].SetPoint = 0;
144
      else                 Motor[i].SetPoint = motorTest[i];
145
    }
146
    if(motorTestActive) motorTestActive--;
147
  }
148
 
149
  /*
1645 - 150
  DebugOut.Analog[] = Motor[0].SetPoint; // Front
151
  DebugOut.Analog[] = Motor[1].SetPoint; // Rear
152
  DebugOut.Analog[] = Motor[3].SetPoint; // Left
153
  DebugOut.Analog[] = Motor[2].SetPoint; // Right
1612 dongfang 154
  */
155
  // Start I2C Interrupt Mode
156
  I2C_Start(TWI_STATE_MOTOR_TX);
157
}
158
 
159
void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
160
  Ki = 10300 / _Ki;
161
  gyroPFactor = _gyroPFactor;
162
  gyroIFactor = _gyroIFactor;
163
  yawPFactor = _yawPFactor;
164
  yawIFactor = _yawIFactor;
165
}
166
 
167
void setNormalFlightParameters(void) {
168
  setFlightParameters(dynamicParams.IFactor + 1,
169
                      dynamicParams.GyroP + 10,
1621 - 170
                      staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI,
1612 dongfang 171
                      dynamicParams.GyroP + 10,
172
                      dynamicParams.UserParams[6]
173
                      );
174
}
175
 
176
void setStableFlightParameters(void) {
177
  setFlightParameters(33, 90, 120, 90, 120);
178
}
179
 
180
void handleCommands(uint8_t command, uint8_t argument, uint8_t isCommandRepeated) {
181
    if(!(MKFlags & MKFLAG_MOTOR_RUN)) {
182
      if (command == COMMAND_GYROCAL && !isCommandRepeated) {
183
        // Run gyro calibration but do not repeat it.
184
        GRN_OFF;
185
 
186
        // TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough?
187
        // isFlying = 0;
188
        // check roll/pitch stick position
189
        // if pitch stick is top or roll stick is left or right --> change parameter setting
190
        // according to roll/pitch stick position
191
 
192
        if (argument < 6) {
193
          // Gyro calinbration, with or without selecting a new parameter-set.
194
          if(argument > 0 && argument < 6) {
195
            // A valid parameter-set (1..5) was chosen - use it.
196
            setActiveParamSet(argument);
197
          }
198
          ParamSet_ReadFromEEProm(getActiveParamSet());
199
          attitude_setNeutral();
200
          flight_setNeutral();
201
          // Right stick is centered; calibrate it to zero (hmm strictly does not belong here).
202
          // If heading hold is active, do not do it. TODO: We also want to re-set old calibration.
203
          controlMixer_setNeutral(!argument);
204
          beepNumber(getActiveParamSet());
205
        } else if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE) && argument == 7) {
206
          // If right stick is centered and down
207
          compassCalState = 1;
208
          beep(1000);
209
        }
210
      }
211
 
212
      // save the ACC neutral setting to eeprom
213
      else  {
214
        if(command == COMMAND_ACCCAL && !isCommandRepeated) {
215
          // Run gyro and acc. meter calibration but do not repeat it.
216
          GRN_OFF;
217
          analog_calibrateAcc();
218
          attitude_setNeutral();
219
          flight_setNeutral();
220
          controlMixer_setNeutral(1); // Calibrate right stick neutral position.
221
          beepNumber(getActiveParamSet());
222
        }
223
      }
224
    } // end !MOTOR_RUN condition.
225
    if (command == COMMAND_START) {
226
      isFlying = 1; // TODO: Really????
227
      // if (!controlMixer_isCommandRepeated()) {
228
      // attitude_startDynamicCalibration(); // Try sense the effect of the motors on sensors.
229
      MKFlags |= (MKFLAG_MOTOR_RUN | MKFLAG_START); // set flag RUN and START. TODO: Is that START flag used at all???
230
      // } else { // Pilot is holding stick, ever after motor start. Continue to sense the effect of the motors on sensors.
231
      // attitude_continueDynamicCalibration();
232
      // setPointYaw = 0;
233
      // IPartPitch = 0;
234
      // IPartRoll = 0;
235
      // }
236
    } else if (command == COMMAND_STOP) {
237
      isFlying = 0;
238
      MKFlags &= ~(MKFLAG_MOTOR_RUN);
239
    }
240
}
241
 
242
/************************************************************************/
243
/*  Main Flight Control                                                 */
244
/************************************************************************/
245
void flight_control(void) {
246
  int16_t tmp_int;
247
    // Mixer Fractions that are combined for Motor Control
1645 - 248
  int16_t yawTerm, throttleTerm, term[2];
1612 dongfang 249
 
250
  // PID controller variables
1645 - 251
  int16_t PDPart[2], PDPartYaw, PPart[2];
252
  static int32_t IPart[2] = {0,0};
1612 dongfang 253
  static int32_t setPointYaw = 0;
254
 
255
  // Removed. Too complicated, and apparently not necessary with MEMS gyros anyway.
256
  // static int32_t IntegralGyroPitchError = 0, IntegralGyroRollError = 0;
257
  // static int32_t CorrectionPitch, CorrectionRoll;
258
 
259
  static uint16_t emergencyFlightTime;
260
  static int8_t debugDataTimer = 1;
261
 
262
  // High resolution motor values for smoothing of PID motor outputs
263
  static int16_t motorFilters[MAX_MOTORS];
264
 
1645 - 265
  uint8_t i, axis;
1612 dongfang 266
 
267
  // Fire the main flight attitude calculation, including integration of angles.
268
  calculateFlightAttitude();
269
  GRN_ON;
270
 
271
  /*
272
   * TODO: update should: Set the stick variables if good signal, set them to zero if bad.
273
   * Set variables also.
274
   */
275
  controlMixer_update();
276
 
277
  throttleTerm = controlThrottle;
1615 dongfang 278
  if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10;
1612 dongfang 279
 
280
  /************************************************************************/
281
  /* RC-signal is bad                                                     */
282
  /* This part could be abstracted, as having yet another control input   */
283
  /* to the control mixer: An emergency autopilot control.                */
284
  /************************************************************************/
285
  if(controlMixer_getSignalQuality() <= SIGNAL_BAD) {           // the rc-frame signal is not reveived or noisy
286
    RED_ON;
287
    beepRCAlarm();
288
 
289
    if(emergencyFlightTime) {
290
      // continue emergency flight
291
      emergencyFlightTime--;
292
      if(isFlying > 1000) {                    
293
        // We're probably still flying. Descend slowly.
294
        throttleTerm = staticParams.EmergencyGas;  // Set emergency throttle
295
        MKFlags |= (MKFLAG_EMERGENCY_LANDING);     // Set flag for emergency landing
296
        setStableFlightParameters();
297
      } else {
298
        MKFlags &= ~(MKFLAG_MOTOR_RUN);            // Probably not flying, and bad R/C signal. Kill motors.
299
      }
300
    } else {
301
      // end emergency flight (just cut the motors???)
302
      MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_LANDING);
303
    }
304
  } else {
305
    // signal is acceptable
306
    if(controlMixer_getSignalQuality() > SIGNAL_BAD) {
307
      // Reset emergency landing control variables.
308
      MKFlags &= ~(MKFLAG_EMERGENCY_LANDING);  // clear flag for emergency landing
309
      // The time is in whole seconds.
310
      emergencyFlightTime = staticParams.EmergencyGasDuration * 488;
311
    }
312
 
313
    // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying.
314
    if(throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
315
      // increment flight-time counter until overflow.
316
      if(isFlying != 0xFFFF) isFlying++;
317
    } else
318
      /*
319
       * When standing on the ground, do not apply I controls and zero the yaw stick.
320
       * Probably to avoid integration effects that will cause the copter to spin
321
       * or flip when taking off.
322
       */
323
      if(isFlying < 256) {
1645 - 324
            IPart[PITCH] = IPart[ROLL] = 0;
325
            // TODO: Don't stomp on other modules' variables!!!
326
            controlYaw = 0;
327
            if(isFlying == 250) {
328
              updateCompassCourse = 1;
329
              yawAngle = 0;
330
              setPointYaw = 0;
331
        }
1612 dongfang 332
      } else {
1645 - 333
            // DebugOut.Digital[1] = 0;
334
            // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? 
335
            // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe.
336
            MKFlags |= (MKFLAG_FLY);
1612 dongfang 337
      }
338
 
339
    /*
340
     * Get the current command (start/stop motors, calibrate), if any.
341
     */
342
    uint8_t command = controlMixer_getCommand();
343
    uint8_t repeated = controlMixer_isCommandRepeated();
344
    uint8_t argument = controlMixer_getArgument();
345
 
346
    handleCommands(command, argument, repeated);
347
 
348
    // if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
349
    setNormalFlightParameters();
350
    // }
351
  } // end else (not bad signal case)
352
 
353
  /*
354
   * Looping the H&I way basically is just a matter of turning off attitude angle measurement
355
   * by integration (because 300 deg/s gyros are too slow) and turning down the throttle.
356
   * This is the throttle part.
357
   */
358
  if(looping) {
359
    if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit;
360
  }
361
 
362
  /************************************************************************/
363
  /*  Yawing                                                              */
364
  /************************************************************************/
365
  if(abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated
366
    badCompassHeading = 1000;
367
    if(!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) {
368
      updateCompassCourse = 1;
369
    }
370
  }
371
 
372
  setPointYaw = controlYaw;
373
 
374
  // Trim drift of yawAngle with controlYaw.
375
  // TODO: We want NO feedback of control related stuff to the attitude related stuff.
376
  yawAngle -= controlYaw;
377
 
378
  // limit the effect
379
  CHECK_MIN_MAX(yawAngle, -50000, 50000)
380
 
381
    /************************************************************************/
382
    /* Compass is currently not supported.                                  */
383
    /************************************************************************/
384
    /*
385
      if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) {
386
      updateCompass();
387
      }
388
    */
389
 
390
#if defined (USE_MK3MAG)
391
    /************************************************************************/
392
    /* GPS is currently not supported.                                      */
393
    /************************************************************************/
394
    /*
395
      if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) {
396
      GPS_Main();
397
      MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
398
      }
399
      else {
400
      // GPSStickPitch = 0;
401
      // GPSStickRoll = 0;
402
      }
403
    */
404
#endif
1645 - 405
 
406
#define SENSOR_LIMIT  (4096 * 4)
1612 dongfang 407
 
408
    /************************************************************************/
409
    /* Calculate control feedback from angle (gyro integral)                */
410
    /* and angular velocity (gyro signal)                                   */
411
    /************************************************************************/
412
    // The P-part is the P of the PID controller. That's the angle integrals (not rates).
1645 - 413
  for (axis=PITCH; axis<=ROLL; axis++) {
414
    if(looping & (1<<(4+axis))) {
415
      PPart[axis] = 0;
416
    } else { // TODO: Where do the 44000 come from???
417
      PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral
418
    }
419
 
420
    /*
421
     * Now blend in the D-part - proportional to the Differential of the integral = the rate.
422
     * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING
423
     * where pfactor is in [0..1].
424
     */
425
    PDPart[axis] = PPart[axis] + (int32_t)((int32_t)rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING))
426
      + (differential[axis] * (int16_t)dynamicParams.GyroD) / 16;
427
 
428
    CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT);
1612 dongfang 429
  }
430
 
1645 - 431
  PDPartYaw = (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING)
432
    + (int32_t)(yawAngle * yawIFactor) / (2 * (44000 / CONTROL_SCALING));
1612 dongfang 433
 
434
  // limit control feedback
435
  CHECK_MIN_MAX(PDPartYaw,  -SENSOR_LIMIT, SENSOR_LIMIT);
436
 
437
  /*
438
   * Compose throttle term.
439
   * If a Bl-Ctrl is missing, prevent takeoff.
440
   */
441
  if(missingMotor) {
442
    // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
443
    if((isFlying > 1) && (isFlying < 50) && (throttleTerm > 0))
444
      isFlying = 1; // keep within lift off condition
1615 dongfang 445
    throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of
1612 dongfang 446
  }
447
 
448
  /*
449
   * Height control was here.
450
   */
1615 dongfang 451
  if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20);
1645 - 452
  throttleTerm *= CONTROL_SCALING;
1612 dongfang 453
 
454
  /*
455
   * Compose yaw term.
1645 - 456
   * The yaw term is limited: Absolute value is max. = the throttle term / 2.
457
   * However, at low throttle the yaw term is limited to a fixed value,
458
   * and at high throttle it is limited by the throttle reserve (the difference
459
   * between current throttle and maximum throttle).
1612 dongfang 460
   */
1645 - 461
#define MIN_YAWGAS (40 * CONTROL_SCALING)  // yaw also below this gas value
462
  yawTerm = PDPartYaw - setPointYaw * CONTROL_SCALING;
1612 dongfang 463
  // limit yawTerm
464
  if(throttleTerm > MIN_YAWGAS) {
465
    CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2));
466
  } else {
467
    CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
468
  }
469
 
1645 - 470
  tmp_int = staticParams.MaxThrottle * CONTROL_SCALING;
1612 dongfang 471
  CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));
472
 
1645 - 473
  tmp_int = (int32_t)((int32_t)dynamicParams.DynamicStability * (int32_t)(throttleTerm + abs(yawTerm) / 2)) / 64;
1612 dongfang 474
 
1645 - 475
  for (axis=PITCH; axis<=ROLL; axis++) {  
476
    /*
477
     * Compose pitch and roll terms. This is finally where the sticks come into play.
478
     */
479
    if(gyroIFactor) {
480
      // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos.
481
      // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time.
482
      // TODO: Find out why this seems to be proportional to stick position - not integrating it at all.
483
      IPart[axis] += PPart[axis] - control[axis]; // Integrate difference between P part (the angle) and the stick pos.
484
    } else {
485
      // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos.
486
      // To keep up with a full stick PDPart should be about 156...
487
      IPart[axis] += PDPart[axis] - control[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
488
    }
489
 
490
    // TODO: From which planet comes the 16000?
491
    CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L));
492
    // Add (P, D) parts minus stick pos. to the scaled-down I part.
493
    term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki;    // PID-controller for pitch
494
 
495
    /*
496
     * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).
497
     * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity
498
     * (max. pitch or roll term is the throttle value).
499
     * TODO: Why a growing function of yaw?
500
     */
501
    CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int);
502
  }
1612 dongfang 503
 
504
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
505
  // Universal Mixer
1645 - 506
  // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
1612 dongfang 507
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
508
  for(i = 0; i < MAX_MOTORS; i++) {
509
    int16_t tmp;
510
    if(Mixer.Motor[i][MIX_THROTTLE] > 0) { // If a motor has a zero throttle mix, it is not considered.
511
      tmp =  ((int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L;
1645 - 512
      tmp += ((int32_t)term[PITCH]  * Mixer.Motor[i][MIX_PITCH])    / 64L;
513
      tmp += ((int32_t)term[ROLL]   * Mixer.Motor[i][MIX_ROLL])     / 64L;
1612 dongfang 514
      tmp += ((int32_t)yawTerm      * Mixer.Motor[i][MIX_YAW])      / 64L;
515
      motorFilters[i] = motorFilter(tmp, motorFilters[i]);
1645 - 516
      tmp = motorFilters[i] / CONTROL_SCALING;
1615 dongfang 517
      CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle);
1612 dongfang 518
      Motor[i].SetPoint = tmp;
519
    }
520
    else Motor[i].SetPoint = 0;
521
  }
522
 
523
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1645 - 524
  // Debugging
1612 dongfang 525
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
526
  if(!(--debugDataTimer)) {
527
    debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
1645 - 528
    DebugOut.Analog[0]  = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
529
    DebugOut.Analog[1]  = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
1612 dongfang 530
    DebugOut.Analog[2]  = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
531
 
1645 - 532
    // DebugOut.Analog[9]  = setPointYaw;
533
    // DebugOut.Analog[10] = yawIFactor;
534
    // DebugOut.Analog[11] = gyroIFactor;
1634 - 535
    // DebugOut.Analog[12] = RC_getVariable(0);
536
    // DebugOut.Analog[13] = dynamicParams.UserParams[0];
1645 - 537
    // DebugOut.Analog[14] = RC_getVariable(4);
538
    // DebugOut.Analog[15] = dynamicParams.UserParams[4];
1612 dongfang 539
    /* DebugOut.Analog[11] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; */
540
 
541
    // 12..15 are the controls.
542
    // DebugOut.Analog[16] = pitchAxisAcc;
543
    // DebugOut.Analog[17] = rollAxisAcc;
1646 - 544
    DebugOut.Analog[18] = HIRES_GYRO_INTEGRATION_FACTOR;
1612 dongfang 545
 
546
    DebugOut.Analog[19] = throttleTerm;
1645 - 547
    DebugOut.Analog[20] = term[PITCH];
548
    DebugOut.Analog[21] = term[ROLL];
1612 dongfang 549
    DebugOut.Analog[22] = yawTerm;
550
 
1645 - 551
    DebugOut.Analog[23] = PPart[PITCH];     //
552
    DebugOut.Analog[24] = IPart[PITCH] /Ki; // meget meget lille.
553
    DebugOut.Analog[25] = PDPart[PITCH];    // omtrent lig ppart.
1612 dongfang 554
 
1645 - 555
    DebugOut.Analog[26] = accNoisePeak[PITCH];
556
    DebugOut.Analog[27] = accNoisePeak[ROLL];
557
 
558
    DebugOut.Analog[30] = gyroNoisePeak[PITCH];
559
    DebugOut.Analog[31] = gyroNoisePeak[ROLL];
1612 dongfang 560
  }
561
}