Subversion Repositories FlightCtrl

Rev

Rev 2015 | Rev 2019 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1868 - 1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1612 dongfang 2
// + Copyright (c) 04.2007 Holger Buss
1870 - 3
// + Nur für den privaten Gebrauch
1612 dongfang 4
// + www.MikroKopter.com
5
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1870 - 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.
1870 - 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
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1870 - 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
1870 - 17
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
18
// + eindeutig als Ursprung verlinkt werden
1612 dongfang 19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1870 - 20
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
1612 dongfang 21
// + Benutzung auf eigene Gefahr
1870 - 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
1870 - 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)
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
1870 - 47
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
1612 dongfang 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"
1845 - 56
#include "output.h"
1612 dongfang 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"
1775 - 67
#include "commands.h"
1612 dongfang 68
#ifdef USE_MK3MAG
69
#include "gps.h"
70
#endif
71
 
72
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
73
 
74
/*
75
 * These are no longer maintained, just left at 0. The original implementation just summed the acc.
76
 * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???
77
 */
1645 - 78
// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
1612 dongfang 79
 
1872 - 80
uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control
1612 dongfang 81
uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control
82
 
83
// Some integral weight constant...
84
uint16_t Ki = 10300 / 33;
85
 
86
/************************************************************************/
87
/*  Filter for motor value smoothing (necessary???)                     */
88
/************************************************************************/
89
int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
1988 - 90
  switch (staticParams.motorSmoothing) {
1841 - 91
  case 0:
92
    return newvalue;
93
  case 1:
1872 - 94
    return (oldvalue + newvalue) / 2;
1841 - 95
  case 2:
1872 - 96
    if (newvalue > oldvalue)
97
      return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
98
    else
1841 - 99
      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
100
  case 3:
1872 - 101
    if (newvalue < oldvalue)
102
      return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
103
    else
1841 - 104
      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
1872 - 105
  default:
106
    return newvalue;
1841 - 107
  }
1612 dongfang 108
}
109
 
110
/************************************************************************/
111
/*  Neutral Readings                                                    */
112
/************************************************************************/
113
void flight_setNeutral() {
1841 - 114
  MKFlags |= MKFLAG_CALIBRATE;
115
  // not really used here any more.
1960 - 116
  /*
1841 - 117
  dynamicParams.KalmanK = -1;
118
  dynamicParams.KalmanMaxDrift = 0;
119
  dynamicParams.KalmanMaxFusion = 32;
1960 - 120
  */
1841 - 121
  controlMixer_initVariables();
1612 dongfang 122
}
123
 
1872 - 124
void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor,
125
    uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
1841 - 126
  Ki = 10300 / _Ki;
127
  gyroPFactor = _gyroPFactor;
128
  gyroIFactor = _gyroIFactor;
129
  yawPFactor = _yawPFactor;
130
  yawIFactor = _yawIFactor;
1612 dongfang 131
}
132
 
133
void setNormalFlightParameters(void) {
1956 - 134
  setFlightParameters(
135
                      dynamicParams.IFactor,
1960 - 136
                      dynamicParams.gyroP,
137
                      staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI,
138
                      dynamicParams.gyroP,
139
                      dynamicParams.yawIFactor
1956 - 140
                      );
1612 dongfang 141
}
142
 
143
void setStableFlightParameters(void) {
1841 - 144
  setFlightParameters(33, 90, 120, 90, 120);
1612 dongfang 145
}
146
 
147
/************************************************************************/
148
/*  Main Flight Control                                                 */
149
/************************************************************************/
150
void flight_control(void) {
1841 - 151
  int16_t tmp_int;
1872 - 152
  // Mixer Fractions that are combined for Motor Control
1841 - 153
  int16_t yawTerm, throttleTerm, term[2];
1612 dongfang 154
 
1841 - 155
  // PID controller variables
2015 - 156
  int16_t PDPart[2],/* DPart[2],*/ PDPartYaw /*, DPartYaw */;
1872 - 157
  static int32_t IPart[2] = { 0, 0 };
1841 - 158
  static uint16_t emergencyFlightTime;
159
  static int8_t debugDataTimer = 1;
1612 dongfang 160
 
1841 - 161
  // High resolution motor values for smoothing of PID motor outputs
162
  static int16_t motorFilters[MAX_MOTORS];
1612 dongfang 163
 
1841 - 164
  uint8_t i, axis;
1612 dongfang 165
 
1841 - 166
  // Fire the main flight attitude calculation, including integration of angles.
1870 - 167
  // We want that to kick as early as possible, not to delay new AD sampling further.
1841 - 168
  calculateFlightAttitude();
1870 - 169
  controlMixer_update();
1908 - 170
  throttleTerm = controls[CONTROL_THROTTLE];
1870 - 171
 
1841 - 172
  // This check removed. Is done on a per-motor basis, after output matrix multiplication.
1960 - 173
  if (throttleTerm < staticParams.minThrottle + 10)
174
    throttleTerm = staticParams.minThrottle + 10;
175
  else if (throttleTerm > staticParams.maxThrottle - 20)
176
    throttleTerm = (staticParams.maxThrottle - 20);
1612 dongfang 177
 
1841 - 178
  /************************************************************************/
179
  /* RC-signal is bad                                                     */
180
  /* This part could be abstracted, as having yet another control input   */
181
  /* to the control mixer: An emergency autopilot control.                */
182
  /************************************************************************/
1775 - 183
 
1872 - 184
  if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy
1841 - 185
    beepRCAlarm();
1872 - 186
 
187
    if (emergencyFlightTime) {
1841 - 188
      // continue emergency flight
1872 - 189
      emergencyFlightTime--;
190
      if (isFlying > 256) {
191
        // We're probably still flying. Descend slowly.
1960 - 192
        throttleTerm = staticParams.emergencyThrottle; // Set emergency throttle
193
        MKFlags |= (MKFLAG_EMERGENCY_FLIGHT); // Set flag for emergency landing
1872 - 194
        setStableFlightParameters();
1841 - 195
      } else {
1872 - 196
        MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors.
1841 - 197
      }
198
    } else {
199
      // end emergency flight (just cut the motors???)
1960 - 200
      MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_FLIGHT);
1841 - 201
    }
1872 - 202
  } else {
1841 - 203
    // signal is acceptable
1872 - 204
    if (controlMixer_getSignalQuality() > SIGNAL_BAD) {
1841 - 205
      // Reset emergency landing control variables.
1960 - 206
      MKFlags &= ~(MKFLAG_EMERGENCY_FLIGHT); // clear flag for emergency landing
1841 - 207
      // The time is in whole seconds.
1960 - 208
      emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration * 488;
1841 - 209
    }
1612 dongfang 210
 
1841 - 211
    // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying.
1872 - 212
    if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
1841 - 213
      // increment flight-time counter until overflow.
1872 - 214
      if (isFlying != 0xFFFF)
215
        isFlying++;
216
    } else
217
    /*
218
     * When standing on the ground, do not apply I controls and zero the yaw stick.
219
     * Probably to avoid integration effects that will cause the copter to spin
220
     * or flip when taking off.
221
     */
1960 - 222
      if (isFlying < 256) {
223
        IPart[PITCH] = IPart[ROLL] = 0;
224
        // TODO: Don't stomp on other modules' variables!!!
225
        // controlYaw = 0;
226
        PDPartYaw = 0; // instead.
227
        if (isFlying == 250) {
228
          // HC_setGround();
229
          updateCompassCourse = 1;
230
          yawAngleDiff = 0;
231
        }
232
      } else {
233
        // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag?
234
        // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe.
235
        MKFlags |= (MKFLAG_FLY);
1841 - 236
      }
1960 - 237
 
1872 - 238
    commands_handleCommands();
1841 - 239
    setNormalFlightParameters();
240
  } // end else (not bad signal case)
1960 - 241
 
1841 - 242
  /************************************************************************/
243
  /*  Yawing                                                              */
244
  /************************************************************************/
1960 - 245
  if (abs(controls[CONTROL_YAW]) > 4 * staticParams.stickYawP) { // yaw stick is activated
1841 - 246
    ignoreCompassTimer = 1000;
1960 - 247
    if (!(staticParams.bitConfig & CFG_COMPASS_FIX)) {
1841 - 248
      updateCompassCourse = 1;
249
    }
250
  }
1872 - 251
 
1960 - 252
  // yawControlRate = controlYaw;
1841 - 253
  // Trim drift of yawAngleDiff with controlYaw.
254
  // TODO: We want NO feedback of control related stuff to the attitude related stuff.
255
  // This seems to be used as: Difference desired <--> real heading.
1908 - 256
  yawAngleDiff -= controls[CONTROL_YAW];
1872 - 257
 
1841 - 258
  // limit the effect
259
  CHECK_MIN_MAX(yawAngleDiff, -50000, 50000);
1872 - 260
 
1841 - 261
  /************************************************************************/
262
  /* Compass is currently not supported.                                  */
263
  /************************************************************************/
1960 - 264
  if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
1841 - 265
    updateCompass();
266
  }
1872 - 267
 
1805 - 268
#if defined (USE_NAVICTRL)
1841 - 269
  /************************************************************************/
270
  /* GPS is currently not supported.                                      */
271
  /************************************************************************/
272
  if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) {
273
    GPS_Main();
274
    MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
275
  } else {
276
  }
1612 dongfang 277
#endif
1841 - 278
  // end part 1: 750-800 usec.
279
  // start part 3: 350 - 400 usec.
1645 - 280
#define SENSOR_LIMIT  (4096 * 4)
1872 - 281
  /************************************************************************/
1775 - 282
 
1872 - 283
  /* Calculate control feedback from angle (gyro integral)                */
284
  /* and angular velocity (gyro signal)                                   */
285
  /************************************************************************/
286
  // The P-part is the P of the PID controller. That's the angle integrals (not rates).
287
  for (axis = PITCH; axis <= ROLL; axis++) {
2015 - 288
    PDPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral
1960 - 289
 
1841 - 290
    /*
291
     * Now blend in the D-part - proportional to the Differential of the integral = the rate.
292
     * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING
293
     * where pfactor is in [0..1].
294
     */
2015 - 295
    PDPart[axis] += (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
1841 - 296
    CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT);
297
  }
1775 - 298
 
2015 - 299
  PDPartYaw = (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING));
300
  PDPartYaw += (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING);
1872 - 301
 
1841 - 302
  // limit control feedback
1992 - 303
  // CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
1872 - 304
 
1841 - 305
  /*
306
   * Compose throttle term.
307
   * If a Bl-Ctrl is missing, prevent takeoff.
308
   */
1872 - 309
  if (missingMotor) {
1841 - 310
    // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
1872 - 311
    if (isFlying > 1 && isFlying < 50 && throttleTerm > 0)
1841 - 312
      isFlying = 1; // keep within lift off condition
1960 - 313
    throttleTerm = staticParams.minThrottle; // reduce gas to min to avoid lift of
1841 - 314
  }
1612 dongfang 315
 
1841 - 316
  // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
317
  throttleTerm *= CONTROL_SCALING;
1612 dongfang 318
 
1841 - 319
  /*
320
   * Compose yaw term.
321
   * The yaw term is limited: Absolute value is max. = the throttle term / 2.
322
   * However, at low throttle the yaw term is limited to a fixed value,
323
   * and at high throttle it is limited by the throttle reserve (the difference
324
   * between current throttle and maximum throttle).
325
   */
1645 - 326
#define MIN_YAWGAS (40 * CONTROL_SCALING)  // yaw also below this gas value
1908 - 327
  yawTerm = PDPartYaw - controls[CONTROL_YAW] * CONTROL_SCALING;
1841 - 328
  // Limit yawTerm
1955 - 329
  debugOut.digital[0] &= ~DEBUG_CLIP;
1872 - 330
  if (throttleTerm > MIN_YAWGAS) {
331
    if (yawTerm < -throttleTerm / 2) {
1955 - 332
      debugOut.digital[0] |= DEBUG_CLIP;
1872 - 333
      yawTerm = -throttleTerm / 2;
334
    } else if (yawTerm > throttleTerm / 2) {
1955 - 335
      debugOut.digital[0] |= DEBUG_CLIP;
1872 - 336
      yawTerm = throttleTerm / 2;
1841 - 337
    }
338
    //CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2));
339
  } else {
1872 - 340
    if (yawTerm < -MIN_YAWGAS / 2) {
1955 - 341
      debugOut.digital[0] |= DEBUG_CLIP;
1872 - 342
      yawTerm = -MIN_YAWGAS / 2;
343
    } else if (yawTerm > MIN_YAWGAS / 2) {
1955 - 344
      debugOut.digital[0] |= DEBUG_CLIP;
1872 - 345
      yawTerm = MIN_YAWGAS / 2;
1841 - 346
    }
347
    //CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
348
  }
1775 - 349
 
1841 - 350
  // FIXME: Throttle may exceed maxThrottle (there is no check no more).
1960 - 351
  tmp_int = staticParams.maxThrottle * CONTROL_SCALING;
1845 - 352
  if (yawTerm < -(tmp_int - throttleTerm)) {
353
    yawTerm = -(tmp_int - throttleTerm);
1955 - 354
    debugOut.digital[0] |= DEBUG_CLIP;
1845 - 355
  } else if (yawTerm > (tmp_int - throttleTerm)) {
356
    yawTerm = (tmp_int - throttleTerm);
1955 - 357
    debugOut.digital[0] |= DEBUG_CLIP;
1841 - 358
  }
1867 - 359
 
1841 - 360
  // CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));
1955 - 361
  debugOut.digital[1] &= ~DEBUG_CLIP;
1872 - 362
  for (axis = PITCH; axis <= ROLL; axis++) {
1841 - 363
    /*
364
     * Compose pitch and roll terms. This is finally where the sticks come into play.
365
     */
1872 - 366
    if (gyroIFactor) {
1841 - 367
      // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos.
368
      // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time.
369
      // TODO: Find out why this seems to be proportional to stick position - not integrating it at all.
2015 - 370
      IPart[axis] += PDPart[axis] - controls[axis]; // Integrate difference between P part (the angle) and the stick pos.
1841 - 371
    } else {
372
      // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos.
373
      // To keep up with a full stick PDPart should be about 156...
1908 - 374
      IPart[axis] += PDPart[axis] - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
1841 - 375
    }
1612 dongfang 376
 
1960 - 377
    tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability
1872 - 378
        * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
1612 dongfang 379
 
1841 - 380
    // TODO: From which planet comes the 16000?
381
    CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L));
382
    // Add (P, D) parts minus stick pos. to the scaled-down I part.
1908 - 383
    term[axis] = PDPart[axis] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch
1991 - 384
        term[axis] += (dynamicParams.levelCorrection[axis] - 128);
1841 - 385
    /*
386
     * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).
387
     * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity
388
     * (max. pitch or roll term is the throttle value).
389
     * TODO: Why a growing function of yaw?
390
     */
391
    if (term[axis] < -tmp_int) {
1955 - 392
      debugOut.digital[1] |= DEBUG_CLIP;
1841 - 393
    } else if (term[axis] > tmp_int) {
1955 - 394
      debugOut.digital[1] |= DEBUG_CLIP;
1841 - 395
    }
396
    CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int);
397
  }
1775 - 398
 
1841 - 399
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
400
  // Universal Mixer
401
  // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
402
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1612 dongfang 403
 
1976 - 404
  debugOut.analog[3]  = rate_ATT[PITCH];
405
  debugOut.analog[4]  = rate_ATT[ROLL];
406
  debugOut.analog[5]  = yawRate;
407
 
408
  debugOut.analog[6]  = filteredAcc[PITCH];
409
  debugOut.analog[7]  = filteredAcc[ROLL];
410
  debugOut.analog[8]  = filteredAcc[Z];
411
 
1955 - 412
  debugOut.analog[12] = term[PITCH];
413
  debugOut.analog[13] = term[ROLL];
414
  debugOut.analog[14] = yawTerm;
415
  debugOut.analog[15] = throttleTerm;
1775 - 416
 
1872 - 417
  for (i = 0; i < MAX_MOTORS; i++) {
1874 - 418
    int32_t tmp;
1908 - 419
    uint8_t throttle;
420
 
1960 - 421
    tmp = (int32_t)throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE];
422
    tmp += (int32_t)term[PITCH] * mixerMatrix.motor[i][MIX_PITCH];
423
    tmp += (int32_t)term[ROLL] * mixerMatrix.motor[i][MIX_ROLL];
424
    tmp += (int32_t)yawTerm * mixerMatrix.motor[i][MIX_YAW];
1908 - 425
    tmp = tmp >> 6;
426
    motorFilters[i] = motorFilter(tmp, motorFilters[i]);
427
    // Now we scale back down to a 0..255 range.
428
    tmp = motorFilters[i] / MOTOR_SCALING;
429
 
430
    // So this was the THIRD time a throttle was limited. But should the limitation
431
    // apply to the common throttle signal (the one used for setting the "power" of
432
    // all motors together) or should it limit the throttle set for each motor,
433
    // including mix components of pitch, roll and yaw? I think only the common
434
    // throttle should be limited.
435
    // --> WRONG. This caused motors to stall completely in tight maneuvers.
436
    // Apply to individual signals instead.
437
    CHECK_MIN_MAX(tmp, 1, 255);
438
    throttle = tmp;
439
 
2017 - 440
    // if (i < 4) debugOut.analog[22 + i] = throttle;
1908 - 441
 
1960 - 442
    if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) {
1908 - 443
      motor[i].SetPoint = throttle;
1872 - 444
    } else if (motorTestActive) {
1841 - 445
      motor[i].SetPoint = motorTest[i];
446
    } else {
447
      motor[i].SetPoint = 0;
448
    }
449
  }
1872 - 450
 
1841 - 451
  I2C_Start(TWI_STATE_MOTOR_TX);
1872 - 452
 
1841 - 453
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
454
  // Debugging
455
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1872 - 456
  if (!(--debugDataTimer)) {
1841 - 457
    debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
1955 - 458
    debugOut.analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
459
    debugOut.analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
460
    debugOut.analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
1612 dongfang 461
 
1955 - 462
    debugOut.analog[16] = gyroPFactor;
1841 - 463
  }
1612 dongfang 464
}