Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
1612 dongfang 1
#include <stdlib.h>
2
#include <avr/io.h>
3
#include "eeprom.h"
4
#include "flight.h"
1845 - 5
#include "output.h"
2052 - 6
#include "uart0.h"
1612 dongfang 7
 
8
// Necessary for external control and motor test
9
#include "twimaster.h"
10
#include "attitude.h"
11
#include "controlMixer.h"
1775 - 12
#include "commands.h"
2052 - 13
#include "heightControl.h"
1612 dongfang 14
 
2052 - 15
#ifdef USE_MK3MAG
16
#include "mk3mag.h"
17
#include "compassControl.h"
18
#endif
19
 
1612 dongfang 20
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
21
 
22
/*
23
 * These are no longer maintained, just left at 0. The original implementation just summed the acc.
24
 * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???
25
 */
1645 - 26
// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
1872 - 27
uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control
1612 dongfang 28
uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control
2085 - 29
uint8_t ki;
2055 - 30
int32_t IPart[2];
1612 dongfang 31
 
32
/************************************************************************/
33
/*  Filter for motor value smoothing (necessary???)                     */
34
/************************************************************************/
35
int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
1988 - 36
  switch (staticParams.motorSmoothing) {
1841 - 37
  case 0:
38
    return newvalue;
39
  case 1:
1872 - 40
    return (oldvalue + newvalue) / 2;
1841 - 41
  case 2:
1872 - 42
    if (newvalue > oldvalue)
43
      return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
44
    else
1841 - 45
      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
46
  case 3:
1872 - 47
    if (newvalue < oldvalue)
48
      return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
49
    else
1841 - 50
      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
1872 - 51
  default:
52
    return newvalue;
1841 - 53
  }
1612 dongfang 54
}
55
 
2085 - 56
void flight_setParameters(uint8_t _ki, uint8_t _gyroPFactor,
1872 - 57
    uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
2085 - 58
  ki = _ki;
1841 - 59
  gyroPFactor = _gyroPFactor;
60
  gyroIFactor = _gyroIFactor;
61
  yawPFactor = _yawPFactor;
62
  yawIFactor = _yawIFactor;
1612 dongfang 63
}
64
 
2055 - 65
void flight_setGround() {
66
  // Just reset all I terms.
67
  IPart[PITCH] = IPart[ROLL] = 0;
68
  headingError = 0;
1612 dongfang 69
}
70
 
2055 - 71
void flight_takeOff() {
2058 - 72
  // This is for GPS module to mark home position.
73
  // TODO: What a disgrace, change it.
74
  MKFlags |= MKFLAG_CALIBRATE;
75
 
2055 - 76
  HC_setGround();
77
#ifdef USE_MK3MAG
78
  attitude_resetHeadingToMagnetic();
79
  compass_setTakeoffHeading(heading);
80
#endif
1612 dongfang 81
}
82
 
83
/************************************************************************/
84
/*  Main Flight Control                                                 */
85
/************************************************************************/
86
void flight_control(void) {
2055 - 87
  int16_t tmp_int;
1872 - 88
  // Mixer Fractions that are combined for Motor Control
1841 - 89
  int16_t yawTerm, throttleTerm, term[2];
1612 dongfang 90
 
1841 - 91
  // PID controller variables
2053 - 92
  int16_t PDPart;
1841 - 93
  static int8_t debugDataTimer = 1;
1612 dongfang 94
 
1841 - 95
  // High resolution motor values for smoothing of PID motor outputs
96
  static int16_t motorFilters[MAX_MOTORS];
1612 dongfang 97
 
1841 - 98
  uint8_t i, axis;
1612 dongfang 99
 
1908 - 100
  throttleTerm = controls[CONTROL_THROTTLE];
1870 - 101
 
2055 - 102
  if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
103
    // increment flight-time counter until overflow.
104
    if (isFlying != 0xFFFF)
105
      isFlying++;
106
  }
107
  /*
108
   * When standing on the ground, do not apply I controls and zero the yaw stick.
109
   * Probably to avoid integration effects that will cause the copter to spin
110
   * or flip when taking off.
111
   */
112
  if (isFlying < 256) {
113
    flight_setGround();
114
    if (isFlying == 250)
115
      flight_takeOff();
116
  }
117
 
1841 - 118
  // This check removed. Is done on a per-motor basis, after output matrix multiplication.
1960 - 119
  if (throttleTerm < staticParams.minThrottle + 10)
120
    throttleTerm = staticParams.minThrottle + 10;
121
  else if (throttleTerm > staticParams.maxThrottle - 20)
122
    throttleTerm = (staticParams.maxThrottle - 20);
1612 dongfang 123
 
2055 - 124
  // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
125
  throttleTerm *= CONTROL_SCALING;
1775 - 126
 
2055 - 127
// end part 1: 750-800 usec.
128
// start part 3: 350 - 400 usec.
2051 - 129
#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)
2055 - 130
// This is where control affects the target heading. It also (later) directly controls yaw.
2051 - 131
  headingError -= controls[CONTROL_YAW];
2058 - 132
 
2055 - 133
  if (headingError < -YAW_I_LIMIT)
134
    headingError = -YAW_I_LIMIT;
2058 - 135
  else if (headingError > YAW_I_LIMIT)
2055 - 136
    headingError = YAW_I_LIMIT;
2048 - 137
 
2055 - 138
  PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
139
// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.
140
  PDPart += (int32_t) (yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5);
1872 - 141
 
2055 - 142
  // Lets not limit P and D.
143
// CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
1612 dongfang 144
 
1841 - 145
  /*
146
   * Compose yaw term.
147
   * The yaw term is limited: Absolute value is max. = the throttle term / 2.
148
   * However, at low throttle the yaw term is limited to a fixed value,
149
   * and at high throttle it is limited by the throttle reserve (the difference
150
   * between current throttle and maximum throttle).
151
   */
1645 - 152
#define MIN_YAWGAS (40 * CONTROL_SCALING)  // yaw also below this gas value
2053 - 153
  yawTerm = PDPart - controls[CONTROL_YAW] * CONTROL_SCALING;
2055 - 154
// Limit yawTerm
1955 - 155
  debugOut.digital[0] &= ~DEBUG_CLIP;
1872 - 156
  if (throttleTerm > MIN_YAWGAS) {
157
    if (yawTerm < -throttleTerm / 2) {
1955 - 158
      debugOut.digital[0] |= DEBUG_CLIP;
1872 - 159
      yawTerm = -throttleTerm / 2;
160
    } else if (yawTerm > throttleTerm / 2) {
1955 - 161
      debugOut.digital[0] |= DEBUG_CLIP;
1872 - 162
      yawTerm = throttleTerm / 2;
1841 - 163
    }
164
  } else {
1872 - 165
    if (yawTerm < -MIN_YAWGAS / 2) {
1955 - 166
      debugOut.digital[0] |= DEBUG_CLIP;
1872 - 167
      yawTerm = -MIN_YAWGAS / 2;
168
    } else if (yawTerm > MIN_YAWGAS / 2) {
1955 - 169
      debugOut.digital[0] |= DEBUG_CLIP;
1872 - 170
      yawTerm = MIN_YAWGAS / 2;
1841 - 171
    }
172
  }
1775 - 173
 
1960 - 174
  tmp_int = staticParams.maxThrottle * CONTROL_SCALING;
2055 - 175
 
1845 - 176
  if (yawTerm < -(tmp_int - throttleTerm)) {
177
    yawTerm = -(tmp_int - throttleTerm);
1955 - 178
    debugOut.digital[0] |= DEBUG_CLIP;
1845 - 179
  } else if (yawTerm > (tmp_int - throttleTerm)) {
180
    yawTerm = (tmp_int - throttleTerm);
1955 - 181
    debugOut.digital[0] |= DEBUG_CLIP;
1841 - 182
  }
1867 - 183
 
1955 - 184
  debugOut.digital[1] &= ~DEBUG_CLIP;
2053 - 185
 
2055 - 186
  tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + (abs(yawTerm) >> 1)) >> 6);
187
  //tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
2053 - 188
 
189
  /************************************************************************/
190
  /* Calculate control feedback from angle (gyro integral)                */
191
  /* and angular velocity (gyro signal)                                   */
192
  /************************************************************************/
193
  // The P-part is the P of the PID controller. That's the angle integrals (not rates).
1872 - 194
  for (axis = PITCH; axis <= ROLL; axis++) {
2055 - 195
    PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
2053 - 196
    // In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.
197
    // In HH mode, the I part is summed from P and D of gyros minus stick.
1872 - 198
    if (gyroIFactor) {
2058 - 199
      int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2);
2092 - 200
      //if (axis == 0) debugOut.analog[28] = iDiff;
2055 - 201
      PDPart += iDiff;
2053 - 202
      IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
1841 - 203
    } else {
2053 - 204
      IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
1841 - 205
    }
1612 dongfang 206
 
2086 - 207
    // So (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4) +
208
    // attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
209
    // We can ignore the rate: attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
210
    // That is: attitudeAngle[degrees] * gyroIFactor/4 - controls[axis]
211
    // So attitude attained at attitudeAngle[degrees] * gyroIFactor/4 == controls[axis]
212
 
2055 - 213
    // With normal Ki, limit I parts to +/- 205 (of about 1024)
2053 - 214
    if (IPart[axis] < -64000) {
215
      IPart[axis] = -64000;
2052 - 216
      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
2053 - 217
    } else if (IPart[axis] > 64000) {
218
      IPart[axis] = 64000;
2052 - 219
      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
220
    }
221
 
2058 - 222
    PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
223
 
2085 - 224
    term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * ki) >> 14);
2055 - 225
    term[axis] += (dynamicParams.levelCorrection[axis] - 128);
226
 
227
    /*
1841 - 228
     * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).
229
     * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity
230
     * (max. pitch or roll term is the throttle value).
2057 - 231
     * OOPS: Is not applied at all.
1841 - 232
     * TODO: Why a growing function of yaw?
233
     */
234
    if (term[axis] < -tmp_int) {
1955 - 235
      debugOut.digital[1] |= DEBUG_CLIP;
2055 - 236
      term[axis] = -tmp_int;
1841 - 237
    } else if (term[axis] > tmp_int) {
1955 - 238
      debugOut.digital[1] |= DEBUG_CLIP;
2055 - 239
      term[axis] = tmp_int;
1841 - 240
    }
241
  }
1775 - 242
 
1841 - 243
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
244
  // Universal Mixer
245
  // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
246
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1612 dongfang 247
 
2055 - 248
  if (!(--debugDataTimer)) {
249
    debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
250
    debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg
251
    debugOut.analog[1] = attitude[ROLL]  / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg
252
    debugOut.analog[2] = heading / GYRO_DEG_FACTOR_YAW;
1976 - 253
 
2055 - 254
    debugOut.analog[3] = rate_ATT[PITCH];
255
    debugOut.analog[4] = rate_ATT[ROLL];
256
    debugOut.analog[5] = yawRate;
257
  }
1976 - 258
 
2055 - 259
  debugOut.analog[8] = yawTerm;
260
  debugOut.analog[9] = throttleTerm;
1775 - 261
 
2058 - 262
  //debugOut.analog[16] = gyroActivity;
2055 - 263
 
1872 - 264
  for (i = 0; i < MAX_MOTORS; i++) {
1874 - 265
    int32_t tmp;
1908 - 266
    uint8_t throttle;
267
 
2055 - 268
    tmp = (int32_t) throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE];
269
    tmp += (int32_t) term[PITCH] * mixerMatrix.motor[i][MIX_PITCH];
270
    tmp += (int32_t) term[ROLL] * mixerMatrix.motor[i][MIX_ROLL];
271
    tmp += (int32_t) yawTerm * mixerMatrix.motor[i][MIX_YAW];
1908 - 272
    tmp = tmp >> 6;
273
    motorFilters[i] = motorFilter(tmp, motorFilters[i]);
274
    // Now we scale back down to a 0..255 range.
275
    tmp = motorFilters[i] / MOTOR_SCALING;
276
 
277
    // So this was the THIRD time a throttle was limited. But should the limitation
278
    // apply to the common throttle signal (the one used for setting the "power" of
279
    // all motors together) or should it limit the throttle set for each motor,
280
    // including mix components of pitch, roll and yaw? I think only the common
281
    // throttle should be limited.
282
    // --> WRONG. This caused motors to stall completely in tight maneuvers.
283
    // Apply to individual signals instead.
284
    CHECK_MIN_MAX(tmp, 1, 255);
285
    throttle = tmp;
286
 
2073 - 287
    /*
2055 - 288
    if (i < 4)
289
      debugOut.analog[10 + i] = throttle;
2073 - 290
      */
1908 - 291
 
1960 - 292
    if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) {
2035 - 293
      motor[i].throttle = throttle;
1872 - 294
    } else if (motorTestActive) {
2035 - 295
      motor[i].throttle = motorTest[i];
1841 - 296
    } else {
2035 - 297
      motor[i].throttle = 0;
1841 - 298
    }
299
  }
1872 - 300
 
1841 - 301
  I2C_Start(TWI_STATE_MOTOR_TX);
1612 dongfang 302
}