Subversion Repositories FlightCtrl

Rev

Rev 2160 | 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;
2055 - 75
  HC_setGround();
76
#ifdef USE_MK3MAG
77
  attitude_resetHeadingToMagnetic();
78
  compass_setTakeoffHeading(heading);
79
#endif
1612 dongfang 80
}
81
 
82
/************************************************************************/
83
/*  Main Flight Control                                                 */
84
/************************************************************************/
85
void flight_control(void) {
2055 - 86
  int16_t tmp_int;
1872 - 87
  // Mixer Fractions that are combined for Motor Control
1841 - 88
  int16_t yawTerm, throttleTerm, term[2];
1612 dongfang 89
 
1841 - 90
  // PID controller variables
2053 - 91
  int16_t PDPart;
1841 - 92
  static int8_t debugDataTimer = 1;
1612 dongfang 93
 
1841 - 94
  // High resolution motor values for smoothing of PID motor outputs
95
  static int16_t motorFilters[MAX_MOTORS];
1612 dongfang 96
 
1841 - 97
  uint8_t i, axis;
1612 dongfang 98
 
1908 - 99
  throttleTerm = controls[CONTROL_THROTTLE];
1870 - 100
 
2055 - 101
  if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
102
    // increment flight-time counter until overflow.
103
    if (isFlying != 0xFFFF)
104
      isFlying++;
105
  }
106
  /*
107
   * When standing on the ground, do not apply I controls and zero the yaw stick.
108
   * Probably to avoid integration effects that will cause the copter to spin
109
   * or flip when taking off.
110
   */
111
  if (isFlying < 256) {
112
    flight_setGround();
113
    if (isFlying == 250)
114
      flight_takeOff();
115
  }
116
 
1841 - 117
  // This check removed. Is done on a per-motor basis, after output matrix multiplication.
1960 - 118
  if (throttleTerm < staticParams.minThrottle + 10)
119
    throttleTerm = staticParams.minThrottle + 10;
120
  else if (throttleTerm > staticParams.maxThrottle - 20)
121
    throttleTerm = (staticParams.maxThrottle - 20);
1612 dongfang 122
 
2055 - 123
  // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
124
  throttleTerm *= CONTROL_SCALING;
1775 - 125
 
2055 - 126
// end part 1: 750-800 usec.
127
// start part 3: 350 - 400 usec.
2051 - 128
#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)
2055 - 129
// This is where control affects the target heading. It also (later) directly controls yaw.
2051 - 130
  headingError -= controls[CONTROL_YAW];
2058 - 131
 
2055 - 132
  if (headingError < -YAW_I_LIMIT)
133
    headingError = -YAW_I_LIMIT;
2058 - 134
  else if (headingError > YAW_I_LIMIT)
2055 - 135
    headingError = YAW_I_LIMIT;
2048 - 136
 
2055 - 137
  PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
138
// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.
139
  PDPart += (int32_t) (yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5);
1872 - 140
 
2055 - 141
  // Lets not limit P and D.
142
// CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
1612 dongfang 143
 
1841 - 144
  /*
145
   * Compose yaw term.
146
   * The yaw term is limited: Absolute value is max. = the throttle term / 2.
147
   * However, at low throttle the yaw term is limited to a fixed value,
148
   * and at high throttle it is limited by the throttle reserve (the difference
149
   * between current throttle and maximum throttle).
150
   */
1645 - 151
#define MIN_YAWGAS (40 * CONTROL_SCALING)  // yaw also below this gas value
2053 - 152
  yawTerm = PDPart - controls[CONTROL_YAW] * CONTROL_SCALING;
2055 - 153
// Limit yawTerm
1955 - 154
  debugOut.digital[0] &= ~DEBUG_CLIP;
1872 - 155
  if (throttleTerm > MIN_YAWGAS) {
156
    if (yawTerm < -throttleTerm / 2) {
1955 - 157
      debugOut.digital[0] |= DEBUG_CLIP;
1872 - 158
      yawTerm = -throttleTerm / 2;
159
    } else if (yawTerm > throttleTerm / 2) {
1955 - 160
      debugOut.digital[0] |= DEBUG_CLIP;
1872 - 161
      yawTerm = throttleTerm / 2;
1841 - 162
    }
163
  } else {
1872 - 164
    if (yawTerm < -MIN_YAWGAS / 2) {
1955 - 165
      debugOut.digital[0] |= DEBUG_CLIP;
1872 - 166
      yawTerm = -MIN_YAWGAS / 2;
167
    } else if (yawTerm > MIN_YAWGAS / 2) {
1955 - 168
      debugOut.digital[0] |= DEBUG_CLIP;
1872 - 169
      yawTerm = MIN_YAWGAS / 2;
1841 - 170
    }
171
  }
1775 - 172
 
1960 - 173
  tmp_int = staticParams.maxThrottle * CONTROL_SCALING;
2055 - 174
 
1845 - 175
  if (yawTerm < -(tmp_int - throttleTerm)) {
176
    yawTerm = -(tmp_int - throttleTerm);
1955 - 177
    debugOut.digital[0] |= DEBUG_CLIP;
1845 - 178
  } else if (yawTerm > (tmp_int - throttleTerm)) {
179
    yawTerm = (tmp_int - throttleTerm);
1955 - 180
    debugOut.digital[0] |= DEBUG_CLIP;
1841 - 181
  }
1867 - 182
 
1955 - 183
  debugOut.digital[1] &= ~DEBUG_CLIP;
2053 - 184
 
2055 - 185
  tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + (abs(yawTerm) >> 1)) >> 6);
186
  //tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
2053 - 187
 
188
  /************************************************************************/
189
  /* Calculate control feedback from angle (gyro integral)                */
190
  /* and angular velocity (gyro signal)                                   */
191
  /************************************************************************/
192
  // The P-part is the P of the PID controller. That's the angle integrals (not rates).
1872 - 193
  for (axis = PITCH; axis <= ROLL; axis++) {
2055 - 194
    PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
2053 - 195
    // In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.
196
    // In HH mode, the I part is summed from P and D of gyros minus stick.
1872 - 197
    if (gyroIFactor) {
2058 - 198
      int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2);
2092 - 199
      //if (axis == 0) debugOut.analog[28] = iDiff;
2055 - 200
      PDPart += iDiff;
2053 - 201
      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 - 202
    } else {
2053 - 203
      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 - 204
    }
1612 dongfang 205
 
2086 - 206
    // So (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4) +
207
    // attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
208
    // We can ignore the rate: attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
209
    // That is: attitudeAngle[degrees] * gyroIFactor/4 - controls[axis]
210
    // So attitude attained at attitudeAngle[degrees] * gyroIFactor/4 == controls[axis]
211
 
2055 - 212
    // With normal Ki, limit I parts to +/- 205 (of about 1024)
2053 - 213
    if (IPart[axis] < -64000) {
214
      IPart[axis] = -64000;
2052 - 215
      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
2053 - 216
    } else if (IPart[axis] > 64000) {
217
      IPart[axis] = 64000;
2052 - 218
      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
219
    }
220
 
2058 - 221
    PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
222
 
2085 - 223
    term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * ki) >> 14);
2055 - 224
    term[axis] += (dynamicParams.levelCorrection[axis] - 128);
225
 
226
    /*
1841 - 227
     * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).
228
     * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity
229
     * (max. pitch or roll term is the throttle value).
2057 - 230
     * OOPS: Is not applied at all.
1841 - 231
     * TODO: Why a growing function of yaw?
232
     */
233
    if (term[axis] < -tmp_int) {
1955 - 234
      debugOut.digital[1] |= DEBUG_CLIP;
2055 - 235
      term[axis] = -tmp_int;
1841 - 236
    } else if (term[axis] > tmp_int) {
1955 - 237
      debugOut.digital[1] |= DEBUG_CLIP;
2055 - 238
      term[axis] = tmp_int;
1841 - 239
    }
240
  }
1775 - 241
 
1841 - 242
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
243
  // Universal Mixer
244
  // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
245
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1612 dongfang 246
 
2055 - 247
  if (!(--debugDataTimer)) {
248
    debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
249
    debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg
250
    debugOut.analog[1] = attitude[ROLL]  / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg
251
    debugOut.analog[2] = heading / GYRO_DEG_FACTOR_YAW;
1976 - 252
 
2160 - 253
    debugOut.analog[16] = acc[PITCH];
254
    debugOut.analog[17] = acc[ROLL];
255
 
2055 - 256
    debugOut.analog[3] = rate_ATT[PITCH];
257
    debugOut.analog[4] = rate_ATT[ROLL];
258
    debugOut.analog[5] = yawRate;
2164 - 259
 
260
    debugOut.analog[6] = getAngleEstimateFromAcc(PITCH) / (int32_t)GYRO_DEG_FACTOR_PITCHROLL;
261
    debugOut.analog[7] = getAngleEstimateFromAcc(ROLL) / (int32_t)GYRO_DEG_FACTOR_PITCHROLL;
262
    debugOut.analog[8] = acc[Z];
263
 
264
    debugOut.analog[9] = controls[CONTROL_PITCH];
265
    debugOut.analog[10] = controls[CONTROL_ROLL];
2055 - 266
  }
1976 - 267
 
2160 - 268
  /*
269
  debugOut.analog[6] = term[PITCH];
270
  debugOut.analog[7] = term[ROLL];
2055 - 271
  debugOut.analog[8] = yawTerm;
272
  debugOut.analog[9] = throttleTerm;
2160 - 273
  */
1775 - 274
 
1872 - 275
  for (i = 0; i < MAX_MOTORS; i++) {
1874 - 276
    int32_t tmp;
1908 - 277
    uint8_t throttle;
278
 
2158 - 279
    tmp = (int32_t) throttleTerm * motorMixer.matrix[i][MIX_THROTTLE];
280
    tmp += (int32_t) term[PITCH] * motorMixer.matrix[i][MIX_PITCH];
281
    tmp += (int32_t) term[ROLL] * motorMixer.matrix[i][MIX_ROLL];
282
    tmp += (int32_t) yawTerm * motorMixer.matrix[i][MIX_YAW];
1908 - 283
    tmp = tmp >> 6;
284
    motorFilters[i] = motorFilter(tmp, motorFilters[i]);
285
    // Now we scale back down to a 0..255 range.
286
    tmp = motorFilters[i] / MOTOR_SCALING;
287
 
288
    // So this was the THIRD time a throttle was limited. But should the limitation
289
    // apply to the common throttle signal (the one used for setting the "power" of
290
    // all motors together) or should it limit the throttle set for each motor,
291
    // including mix components of pitch, roll and yaw? I think only the common
292
    // throttle should be limited.
293
    // --> WRONG. This caused motors to stall completely in tight maneuvers.
294
    // Apply to individual signals instead.
295
    CHECK_MIN_MAX(tmp, 1, 255);
296
    throttle = tmp;
297
 
2073 - 298
    /*
2055 - 299
    if (i < 4)
300
      debugOut.analog[10 + i] = throttle;
2073 - 301
      */
1908 - 302
 
2158 - 303
    if ((MKFlags & MKFLAG_MOTOR_RUN) && motorMixer.matrix[i][MIX_THROTTLE] > 0) {
2035 - 304
      motor[i].throttle = throttle;
1872 - 305
    } else if (motorTestActive) {
2035 - 306
      motor[i].throttle = motorTest[i];
1841 - 307
    } else {
2035 - 308
      motor[i].throttle = 0;
1841 - 309
    }
310
  }
1872 - 311
 
1841 - 312
  I2C_Start(TWI_STATE_MOTOR_TX);
1612 dongfang 313
}