Subversion Repositories FlightCtrl

Rev

Rev 2164 | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2164 Rev 2189
Line 1... Line 1...
1
#include <stdlib.h>
1
#include <stdlib.h>
2
#include <avr/io.h>
2
#include <avr/io.h>
-
 
3
#include <math.h>
3
#include "eeprom.h"
4
//#include "eeprom.h"
4
#include "flight.h"
5
#include "flight.h"
5
#include "output.h"
6
#include "output.h"
6
#include "uart0.h"
7
//#include "uart0.h"
Line 7... Line 8...
7
 
8
 
8
// Necessary for external control and motor test
9
// Necessary for external control and motor test
9
#include "twimaster.h"
10
//#include "twimaster.h"
-
 
11
#include "attitude.h"
10
#include "attitude.h"
12
#include "analog.h"
11
#include "controlMixer.h"
13
#include "controlMixer.h"
12
#include "commands.h"
14
#include "commands.h"
-
 
15
#include "heightControl.h"
-
 
16
#include "definitions.h"
Line 13... Line 17...
13
#include "heightControl.h"
17
#include "debug.h"
14
 
18
 
15
#ifdef USE_MK3MAG
19
#ifdef USE_MK3MAG
16
#include "mk3mag.h"
20
#include "mk3mag.h"
Line 17... Line -...
17
#include "compassControl.h"
-
 
18
#endif
-
 
19
 
-
 
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
 */
-
 
26
// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
21
#include "compassControl.h"
27
uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control
22
#endif
-
 
23
 
-
 
24
int16_t targetHeading;
Line 28... Line 25...
28
uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control
25
int32_t IPart[2];
29
uint8_t ki;
26
FlightMode_t flight_flightMode = FM_UNINITALIZED;
30
int32_t IPart[2];
27
int16_t minThrottle, maxThrottle;
-
 
28
 
31
 
29
/************************************************************************/
32
/************************************************************************/
30
/*  Filter for motor value smoothing (necessary???)                     */
33
/*  Filter for motor value smoothing (necessary???)                     */
31
/************************************************************************/
34
/************************************************************************/
32
/*
35
int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
33
 int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
Line 50... Line 48...
50
      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
48
 return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
51
  default:
49
 default:
52
    return newvalue;
50
 return newvalue;
53
  }
51
 }
54
}
52
 }
-
 
53
 */
Line 55... Line -...
55
 
-
 
56
void flight_setParameters(uint8_t _ki, uint8_t _gyroPFactor,
-
 
57
    uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
-
 
58
  ki = _ki;
-
 
59
  gyroPFactor = _gyroPFactor;
-
 
60
  gyroIFactor = _gyroIFactor;
54
 
61
  yawPFactor = _yawPFactor;
55
void resetIParts(void) {
62
  yawIFactor = _yawIFactor;
56
  IPart[X] = IPart[Y] = 0;
Line -... Line 57...
-
 
57
}
-
 
58
 
-
 
59
void flight_setMode(FlightMode_t _flightMode) {
-
 
60
  if (flight_flightMode != _flightMode) {
-
 
61
    resetIParts();
-
 
62
    flight_flightMode = _flightMode;
-
 
63
  }
-
 
64
}
63
}
65
 
-
 
66
// To be called only when necessary.
-
 
67
void flight_setParameters(void) {
-
 
68
  minThrottle = staticParams.minThrottle << LOG_CONTROL_BYTE_SCALING;
-
 
69
  maxThrottle = staticParams.maxThrottle << LOG_CONTROL_BYTE_SCALING;
-
 
70
}
-
 
71
 
-
 
72
// A heuristic when the yaw attitude cannot be used for yaw control because the airframe is near-vertical or inverted.
-
 
73
// Fum of abs(pitch) and abs(roll) greater than 75 degrees or about 1.3 radians.
-
 
74
uint8_t isHeadingUnaccountable(void) {
64
 
75
  int16_t x = attitude[X];
-
 
76
  if (x<0) x = -x;
65
void flight_setGround() {
77
  int16_t y = attitude[Y];
-
 
78
  if (y<0) y = -y;
-
 
79
  int32_t result = (int32_t)x + y;
-
 
80
  return result >= (long)(1.3 * INT16DEG_PI_FACTOR);
-
 
81
}
66
  // Just reset all I terms.
82
 
-
 
83
void flight_setGround(void) {
67
  IPart[PITCH] = IPart[ROLL] = 0;
84
  resetIParts();
Line 68... Line 85...
68
  headingError = 0;
85
  targetHeading = attitude[Z];
69
}
86
}
70
 
87
 
71
void flight_takeOff() {
88
void flight_takeOff(void) {
72
  // This is for GPS module to mark home position.
89
  // This is for GPS module to mark home position.
73
  // TODO: What a disgrace, change it.
90
  // TODO: What a disgrace, change it.
74
  MKFlags |= MKFLAG_CALIBRATE;
91
  // MKFlags |= MKFLAG_CALIBRATE;
75
  HC_setGround();
92
  HC_setGround();
76
#ifdef USE_MK3MAG
93
#ifdef USE_MK3MAG
77
  attitude_resetHeadingToMagnetic();
94
  attitude_resetHeadingToMagnetic();
Line 78... Line 95...
78
  compass_setTakeoffHeading(heading);
95
  compass_setTakeoffHeading(attitude[YAW]);
79
#endif
96
#endif
80
}
97
}
81
 
98
 
82
/************************************************************************/
-
 
83
/*  Main Flight Control                                                 */
-
 
84
/************************************************************************/
-
 
Line 85... Line 99...
85
void flight_control(void) {
99
/************************************************************************/
86
  int16_t tmp_int;
100
/*  Main Flight Control                                                 */
87
  // Mixer Fractions that are combined for Motor Control
-
 
Line 88... Line 101...
88
  int16_t yawTerm, throttleTerm, term[2];
101
/************************************************************************/
89
 
102
void flight_control(void) {
Line 90... Line 103...
90
  // PID controller variables
103
 
Line 91... Line -...
91
  int16_t PDPart;
-
 
92
  static int8_t debugDataTimer = 1;
-
 
93
 
104
  // PID controller variables
94
  // High resolution motor values for smoothing of PID motor outputs
105
  float PID;
95
  static int16_t motorFilters[MAX_MOTORS];
106
 
96
 
107
  // High resolution motor values for smoothing of PID motor outputs
97
  uint8_t i, axis;
108
  // static int16_t motorFilters[MAX_MOTORS];
98
 
109
 
Line 108... Line 119...
108
   * Probably to avoid integration effects that will cause the copter to spin
119
   * Probably to avoid integration effects that will cause the copter to spin
109
   * or flip when taking off.
120
   * or flip when taking off.
110
   */
121
   */
111
  if (isFlying < 256) {
122
  if (isFlying < 256) {
112
    flight_setGround();
123
    flight_setGround();
113
    if (isFlying == 250)
124
    if (isFlying == 255)
114
      flight_takeOff();
125
      flight_takeOff();
115
  }
126
  }
Line 116... Line 127...
116
 
127
 
117
  // This check removed. Is done on a per-motor basis, after output matrix multiplication.
128
  // This check removed. Is done on a per-motor basis, after output matrix multiplication.
118
  if (throttleTerm < staticParams.minThrottle + 10)
129
  if (throttleTerm < minThrottle)
119
    throttleTerm = staticParams.minThrottle + 10;
130
    throttleTerm = minThrottle;
120
  else if (throttleTerm > staticParams.maxThrottle - 20)
131
  else if (throttleTerm > maxThrottle)
121
    throttleTerm = (staticParams.maxThrottle - 20);
-
 
122
 
-
 
123
  // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
-
 
124
  throttleTerm *= CONTROL_SCALING;
-
 
125
 
-
 
126
// end part 1: 750-800 usec.
-
 
127
// start part 3: 350 - 400 usec.
-
 
128
#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)
-
 
129
// This is where control affects the target heading. It also (later) directly controls yaw.
-
 
Line 130... Line -...
130
  headingError -= controls[CONTROL_YAW];
-
 
131
 
-
 
132
  if (headingError < -YAW_I_LIMIT)
-
 
133
    headingError = -YAW_I_LIMIT;
-
 
134
  else if (headingError > YAW_I_LIMIT)
-
 
135
    headingError = YAW_I_LIMIT;
-
 
136
 
132
    throttleTerm = maxThrottle;
137
  PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
133
 
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);
134
  // This is where control affects the target heading. It also (later) directly controls yaw.
140
 
-
 
Line 141... Line -...
141
  // Lets not limit P and D.
-
 
142
// CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
135
  targetHeading -= ((int32_t)controls[CONTROL_YAW] * YAW_STICK_INTEGRATION_SCALER_LSHIFT16) >> 16;
143
 
-
 
144
  /*
-
 
145
   * Compose yaw term.
136
  int16_t headingError;
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
   */
-
 
151
#define MIN_YAWGAS (40 * CONTROL_SCALING)  // yaw also below this gas value
-
 
152
  yawTerm = PDPart - controls[CONTROL_YAW] * CONTROL_SCALING;
-
 
153
// Limit yawTerm
-
 
154
  debugOut.digital[0] &= ~DEBUG_CLIP;
-
 
155
  if (throttleTerm > MIN_YAWGAS) {
-
 
156
    if (yawTerm < -throttleTerm / 2) {
-
 
157
      debugOut.digital[0] |= DEBUG_CLIP;
137
 
158
      yawTerm = -throttleTerm / 2;
138
  if (isHeadingUnaccountable()) {
159
    } else if (yawTerm > throttleTerm / 2) {
-
 
160
      debugOut.digital[0] |= DEBUG_CLIP;
139
    // inverted flight. Attitude[Z] is off by PI and we should react in the oppisite direction!
161
      yawTerm = throttleTerm / 2;
-
 
162
    }
140
    debugOut.digital[0] |= DEBUG_INVERTED;
163
  } else {
-
 
164
    if (yawTerm < -MIN_YAWGAS / 2) {
-
 
165
      debugOut.digital[0] |= DEBUG_CLIP;
141
    headingError = 0;
166
      yawTerm = -MIN_YAWGAS / 2;
-
 
167
    } else if (yawTerm > MIN_YAWGAS / 2) {
142
  } else {
-
 
143
    debugOut.digital[0] &= ~DEBUG_INVERTED;
-
 
144
    headingError = attitude[Z] - targetHeading;
-
 
145
  }
-
 
146
 
-
 
147
  // Ahaa. Wait. Here is pretty much same check.
-
 
148
  if (headingError < -YAW_I_LIMIT) {
-
 
149
    headingError = -YAW_I_LIMIT;
-
 
150
    targetHeading = attitude[Z] + YAW_I_LIMIT;
168
      debugOut.digital[0] |= DEBUG_CLIP;
151
  } else if (headingError > YAW_I_LIMIT) {
Line -... Line 152...
-
 
152
    headingError = YAW_I_LIMIT;
-
 
153
    targetHeading = attitude[Z] - YAW_I_LIMIT;
-
 
154
  }
-
 
155
 
169
      yawTerm = MIN_YAWGAS / 2;
156
  //debugOut.analog[24] = targetHeading;
Line 170... Line -...
170
    }
-
 
171
  }
157
  //debugOut.analog[25] = attitude[Z];
172
 
-
 
173
  tmp_int = staticParams.maxThrottle * CONTROL_SCALING;
158
  //debugOut.analog[26] = headingError;
174
 
-
 
175
  if (yawTerm < -(tmp_int - throttleTerm)) {
-
 
176
    yawTerm = -(tmp_int - throttleTerm);
-
 
Line 177... Line 159...
177
    debugOut.digital[0] |= DEBUG_CLIP;
159
  //debugOut.analog[27] = positiveDynamic;
-
 
160
  //debugOut.analog[28] = negativeDynamic;
-
 
161
 
Line 178... Line 162...
178
  } else if (yawTerm > (tmp_int - throttleTerm)) {
162
  // Yaw P part.
179
    yawTerm = (tmp_int - throttleTerm);
163
  PID = ((int32_t)gyro_control[Z] * YAW_RATE_SCALER_LSHIFT16 * dynamicParams.yawGyroP) >> 16;
Line 180... Line 164...
180
    debugOut.digital[0] |= DEBUG_CLIP;
164
 
181
  }
165
  if (flight_flightMode != FM_RATE) {
182
 
166
    PID += ((int32_t)headingError * ATT_P_SCALER_LSHIFT16 * dynamicParams.yawGyroI) >> 16;
183
  debugOut.digital[1] &= ~DEBUG_CLIP;
167
  }
184
 
-
 
185
  tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + (abs(yawTerm) >> 1)) >> 6);
168
 
186
  //tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
169
  yawTerm = PID + controls[CONTROL_YAW];
187
 
-
 
188
  /************************************************************************/
170
  // yawTerm = limitYawToThrottle(yawTerm);
189
  /* Calculate control feedback from angle (gyro integral)                */
171
 
190
  /* and angular velocity (gyro signal)                                   */
172
  /************************************************************************/
191
  /************************************************************************/
173
  /* Calculate control feedback from angle (gyro integral)                */
192
  // The P-part is the P of the PID controller. That's the angle integrals (not rates).
174
  /* and angular velocity (gyro signal)                                   */
-
 
175
  /************************************************************************/
193
  for (axis = PITCH; axis <= ROLL; axis++) {
176
  for (uint8_t axis = CONTROL_ROLL; axis <= CONTROL_PITCH; axis++) {
194
    PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
177
    if (flight_flightMode == FM_RETURN_TO_LEVEL) {
-
 
178
      // Control.
-
 
179
      // The P part is attitude error. 
195
    // In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.
180
      PID = (((int32_t)attitude[axis] * ATT_P_SCALER_LSHIFT16 * dynamicParams.attGyroP) >> 16) + controls[axis];
-
 
181
      // The I part is attitude error integral.
-
 
182
      IPart[axis] += PID;
196
    // In HH mode, the I part is summed from P and D of gyros minus stick.
183
      // The D part is rate.
Line 197... Line -...
197
    if (gyroIFactor) {
-
 
198
      int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2);
-
 
199
      //if (axis == 0) debugOut.analog[28] = iDiff;
-
 
200
      PDPart += iDiff;
184
      PID += ((int32_t)gyro_control[axis] * RATE_P_SCALER_LSHIFT16 * dynamicParams.attGyroD) >> 16;
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.
-
 
202
    } else {
-
 
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.
-
 
204
    }
185
    } else {
205
 
-
 
206
    // So (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4) +
-
 
207
    // attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
186
      // We want: Normal stick gain, full stick deflection should drive gyros halfway towards saturation.
208
    // We can ignore the rate: attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
187
      // If that is not enough, then fully towards saturation.
209
    // That is: attitudeAngle[degrees] * gyroIFactor/4 - controls[axis]
-
 
210
    // So attitude attained at attitudeAngle[degrees] * gyroIFactor/4 == controls[axis]
-
 
211
 
-
 
212
    // With normal Ki, limit I parts to +/- 205 (of about 1024)
-
 
213
    if (IPart[axis] < -64000) {
-
 
214
      IPart[axis] = -64000;
-
 
215
      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
188
      PID = (((int32_t)gyro_control[axis] * RATE_P_SCALER_LSHIFT16 * dynamicParams.rateGyroP) >> 16) + controls[axis];
Line 216... Line -...
216
    } else if (IPart[axis] > 64000) {
-
 
217
      IPart[axis] = 64000;
-
 
218
      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
-
 
219
    }
-
 
220
 
-
 
221
    PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
-
 
222
 
-
 
223
    term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * ki) >> 14);
-
 
224
    term[axis] += (dynamicParams.levelCorrection[axis] - 128);
189
      IPart[axis] += PID;
225
 
-
 
226
    /*
-
 
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).
-
 
230
     * OOPS: Is not applied at all.
190
      PID -= ((int32_t)gyroD[axis] * dynamicParams.rateGyroD) >> 8;
Line -... Line 191...
-
 
191
    }
-
 
192
 
-
 
193
    // PDPart += (gyroD[axis] * (int16_t) dynamicParams.gyroD) / 16;
-
 
194
    // Right now, let us ignore I.
231
     * TODO: Why a growing function of yaw?
195
    // term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * ki) >> 14);
232
     */
196
    term[axis] = PID;
233
    if (term[axis] < -tmp_int) {
197
    term[axis] += (dynamicParams.levelCorrection[axis] - 128);
234
      debugOut.digital[1] |= DEBUG_CLIP;
198
 
Line 235... Line -...
235
      term[axis] = -tmp_int;
-
 
236
    } else if (term[axis] > tmp_int) {
-
 
237
      debugOut.digital[1] |= DEBUG_CLIP;
-
 
238
      term[axis] = tmp_int;
-
 
239
    }
-
 
240
  }
-
 
241
 
-
 
242
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
243
  // Universal Mixer
-
 
244
  // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
-
 
245
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
246
 
-
 
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;
-
 
252
 
199
    debugOut.analog[12 + axis] = term[axis];
253
    debugOut.analog[16] = acc[PITCH];
200
  }
254
    debugOut.analog[17] = acc[ROLL];
-
 
255
 
-
 
256
    debugOut.analog[3] = rate_ATT[PITCH];
-
 
257
    debugOut.analog[4] = rate_ATT[ROLL];
-
 
258
    debugOut.analog[5] = yawRate;
201
 
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];
-
 
266
  }
-
 
267
 
-
 
268
  /*
-
 
269
  debugOut.analog[6] = term[PITCH];
-
 
270
  debugOut.analog[7] = term[ROLL];
-
 
271
  debugOut.analog[8] = yawTerm;
-
 
272
  debugOut.analog[9] = throttleTerm;
-
 
273
  */
-
 
274
 
-
 
275
  for (i = 0; i < MAX_MOTORS; i++) {
-
 
276
    int32_t tmp;
-
 
277
    uint8_t throttle;
-
 
278
 
-
 
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];
-
 
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
 
-
 
298
    /*
-
 
299
    if (i < 4)
-
 
300
      debugOut.analog[10 + i] = throttle;
-
 
301
      */
202
  //debugOut.analog[14] = yawTerm;