Subversion Repositories FlightCtrl

Rev

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