Subversion Repositories FlightCtrl

Rev

Rev 1922 | Rev 2099 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1922 Rev 1927
1
#include <stdlib.h>
1
#include <stdlib.h>
2
#include <avr/io.h>
2
#include <avr/io.h>
3
#include "eeprom.h"
3
#include "eeprom.h"
4
#include "flight.h"
4
#include "flight.h"
5
#include "output.h"
5
#include "output.h"
6
 
6
 
7
// Necessary for external control and motor test
7
// Necessary for external control and motor test
8
#include "uart0.h"
8
#include "uart0.h"
9
 
9
 
10
// for scope debugging
10
// for scope debugging
11
// #include "rc.h"
11
// #include "rc.h"
12
 
12
 
13
#include "timer2.h"
13
#include "timer2.h"
14
#include "attitude.h"
14
#include "attitude.h"
15
#include "controlMixer.h"
15
#include "controlMixer.h"
16
#include "commands.h"
16
#include "commands.h"
17
#ifdef USE_MK3MAG
17
#ifdef USE_MK3MAG
18
#include "gps.h"
18
#include "gps.h"
19
#endif
19
#endif
20
 
20
 
21
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
21
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
22
 
22
 
23
/*
23
/*
24
 * These are no longer maintained, just left at 0. The original implementation just summed the acc.
24
 * These are no longer maintained, just left at 0. The original implementation just summed the acc.
25
 * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???
25
 * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???
26
 */
26
 */
27
// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
27
// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
28
 
28
 
29
int8_t pitchPFactor, rollPFactor, yawPFactor;
29
int8_t pitchPFactor, rollPFactor, yawPFactor;
30
int8_t pitchDFactor, rollDFactor, yawDFactor;
30
int8_t pitchDFactor, rollDFactor, yawDFactor;
31
 
31
 
32
int32_t IPart[2] = {0,0};
32
int32_t IPart[2] = {0,0};
33
 
33
 
34
/************************************************************************/
34
/************************************************************************/
35
/*  Filter for motor value smoothing (necessary???)                     */
35
/*  Filter for motor value smoothing (necessary???)                     */
36
/************************************************************************/
36
/************************************************************************/
37
int16_t outputFilter(int16_t newvalue, int16_t oldvalue) {
37
int16_t outputFilter(int16_t newvalue, int16_t oldvalue) {
38
  switch (dynamicParams.UserParams[5]) {
38
  switch (dynamicParams.UserParams[5]) {
39
  case 0:
39
  case 0:
40
    return newvalue;
40
    return newvalue;
41
  case 1:
41
  case 1:
42
    return (oldvalue + newvalue) / 2;
42
    return (oldvalue + newvalue) / 2;
43
  case 2:
43
  case 2:
44
    if (newvalue > oldvalue)
44
    if (newvalue > oldvalue)
45
      return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
45
      return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
46
    else
46
    else
47
      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
47
      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
48
  case 3:
48
  case 3:
49
    if (newvalue < oldvalue)
49
    if (newvalue < oldvalue)
50
      return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
50
      return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
51
    else
51
    else
52
      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
52
      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
53
  default:
53
  default:
54
    return newvalue;
54
    return newvalue;
55
  }
55
  }
56
}
56
}
57
 
57
 
58
/************************************************************************/
58
/************************************************************************/
59
/*  Neutral Readings                                                    */
59
/*  Neutral Readings                                                    */
60
/************************************************************************/
60
/************************************************************************/
61
#define CONTROL_CONFIG_SCALE 10
61
#define CONTROL_CONFIG_SCALE 10
62
 
62
 
63
void flight_setNeutral() {
63
void flight_setNeutral() {
64
  MKFlags |= MKFLAG_CALIBRATE;
64
  MKFlags |= MKFLAG_CALIBRATE;
65
  // not really used here any more.
65
  // not really used here any more.
66
  controlMixer_initVariables();
66
  controlMixer_initVariables();
67
}
67
}
68
 
68
 
69
void setFlightParameters
69
void setFlightParameters
70
(
70
(
71
 uint8_t _pitchPFactor,
71
 uint8_t _pitchPFactor,
72
 uint8_t _rollPFactor,
72
 uint8_t _rollPFactor,
73
 uint8_t _yawPFactor,
73
 uint8_t _yawPFactor,
74
 
74
 
75
 uint8_t _pitchDFactor,
75
 uint8_t _pitchDFactor,
76
 uint8_t _rollDFactor,
76
 uint8_t _rollDFactor,
77
 uint8_t _yawDFactor
77
 uint8_t _yawDFactor
78
 ) {
78
 ) {
79
  pitchPFactor = _pitchPFactor;
79
  pitchPFactor = _pitchPFactor;
80
  rollPFactor = _rollPFactor;
80
  rollPFactor = _rollPFactor;
81
  yawPFactor = _yawPFactor;
81
  yawPFactor = _yawPFactor;
82
 
82
 
83
  pitchDFactor = _pitchDFactor;
83
  pitchDFactor = _pitchDFactor;
84
  rollDFactor = _rollDFactor;
84
  rollDFactor = _rollDFactor;
85
  yawDFactor = _yawDFactor;
85
  yawDFactor = _yawDFactor;
86
}
86
}
87
 
87
 
88
void setNormalFlightParameters(void) {
88
void setNormalFlightParameters(void) {
89
  setFlightParameters
89
  setFlightParameters
90
    (
90
    (
91
     dynamicParams.GyroPitchP / CONTROL_CONFIG_SCALE,     // 12 seems good
91
     dynamicParams.GyroPitchP / CONTROL_CONFIG_SCALE,     // 12 seems good
92
     dynamicParams.GyroRollP / CONTROL_CONFIG_SCALE,      // 9 seems good
92
     dynamicParams.GyroRollP / CONTROL_CONFIG_SCALE,      // 9 seems good
93
     dynamicParams.GyroYawP / (CONTROL_CONFIG_SCALE/2),   // 24 seems too little
93
     dynamicParams.GyroYawP / (CONTROL_CONFIG_SCALE/2),   // 24 seems too little
94
 
94
 
95
     dynamicParams.GyroPitchD / CONTROL_CONFIG_SCALE,
95
     dynamicParams.GyroPitchD / CONTROL_CONFIG_SCALE,
96
     dynamicParams.GyroRollD / CONTROL_CONFIG_SCALE,
96
     dynamicParams.GyroRollD / CONTROL_CONFIG_SCALE,
97
     dynamicParams.GyroYawD / CONTROL_CONFIG_SCALE
97
     dynamicParams.GyroYawD / CONTROL_CONFIG_SCALE
98
     );
98
     );
99
}
99
}
100
 
100
 
101
void setStableFlightParameters(void) {
101
void setStableFlightParameters(void) {
102
  setFlightParameters(0, 0, 0, 0, 0, 0);
102
  setFlightParameters(0, 0, 0, 0, 0, 0);
103
}
103
}
104
 
104
 
105
/************************************************************************/
105
/************************************************************************/
106
/*  Main Flight Control                                                 */
106
/*  Main Flight Control                                                 */
107
/************************************************************************/
107
/************************************************************************/
108
void flight_control(void) {
108
void flight_control(void) {
109
  // Mixer Fractions that are combined for Motor Control
109
  // Mixer Fractions that are combined for Motor Control
110
  int16_t yawTerm, throttleTerm, term[2];
110
  int16_t yawTerm, throttleTerm, term[2];
111
   
111
   
112
  // PID controller variables
112
  // PID controller variables
113
  int16_t PDPart[2], PDPartYaw;
113
  int16_t PDPart[2], PDPartYaw;
114
 
114
 
115
  static int8_t debugDataTimer = 1;
115
  static int8_t debugDataTimer = 1;
116
 
116
 
117
  // High resolution motor values for smoothing of PID motor outputs
117
  // High resolution motor values for smoothing of PID motor outputs
118
  static int16_t outputFilters[MAX_OUTPUTS];
118
  static int16_t outputFilters[MAX_OUTPUTS];
119
 
119
 
120
  uint8_t i;
120
  uint8_t i;
121
 
121
 
122
  // Fire the main flight attitude calculation, including integration of angles.
122
  // Fire the main flight attitude calculation, including integration of angles.
123
  // We want that to kick as early as possible, not to delay new AD sampling further.
123
  // We want that to kick as early as possible, not to delay new AD sampling further.
124
  calculateFlightAttitude();
124
  calculateFlightAttitude();
125
  controlMixer_update();
125
  controlMixer_update();
126
  throttleTerm = control[CONTROL_THROTTLE];
126
  throttleTerm = control[CONTROL_THROTTLE];
127
 
127
 
128
  /************************************************************************/
128
  /************************************************************************/
129
  /* RC-signal is bad                                                     */
129
  /* RC-signal is bad                                                     */
130
  /************************************************************************/
130
  /************************************************************************/
131
 
131
 
132
  if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not received or noisy
132
  if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not received or noisy
133
    RED_ON;
133
    RED_ON;
134
    beepRCAlarm();
134
    beepRCAlarm();
135
    setStableFlightParameters();
135
    setStableFlightParameters();
136
  } else {
136
  } else {
137
    commands_handleCommands();
137
    commands_handleCommands();
138
    setNormalFlightParameters();
138
    setNormalFlightParameters();
139
  }
139
  }
140
   
140
   
141
  /************************************************************************/
141
  /************************************************************************/
142
  /* Calculate control feedback from angle (gyro integral)                */
142
  /* Calculate control feedback from angle (gyro integral)                */
143
  /* and angular velocity (gyro signal)                                   */
143
  /* and angular velocity (gyro signal)                                   */
144
  /************************************************************************/
144
  /************************************************************************/
145
  PDPart[PITCH] = ((int32_t) rate_PID[PITCH] * pitchPFactor /
145
  PDPart[PITCH] = ((int32_t) rate_PID[PITCH] * pitchPFactor /
146
                  (256L / CONTROL_SCALING))
146
                  (256L / CONTROL_SCALING))
147
  + (differential[PITCH] * (int16_t) dynamicParams.GyroPitchD) / 16;
147
  + (differential[PITCH] * (int16_t) dynamicParams.GyroPitchD) / 16;
148
 
148
 
149
  PDPart[ROLL] = ((int32_t) rate_PID[ROLL] * rollPFactor /
149
  PDPart[ROLL] = ((int32_t) rate_PID[ROLL] * rollPFactor /
150
                  (256L / CONTROL_SCALING))
150
                  (256L / CONTROL_SCALING))
151
  + (differential[ROLL] * (int16_t) dynamicParams.GyroRollD) / 16;
151
  + (differential[ROLL] * (int16_t) dynamicParams.GyroRollD) / 16;
152
 
152
 
153
  PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING)
153
  PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING)
154
  + (differential[YAW] * (int16_t) dynamicParams.GyroYawD) / 16;
154
  + (differential[YAW] * (int16_t) dynamicParams.GyroYawD) / 16;
155
 
155
 
156
  /************************************************************************/
156
  /************************************************************************/
157
  /* Stick signals are positive and gyros are negative...                 */
157
  /* Stick signals are positive and gyros are negative...                 */
158
  /************************************************************************/
158
  /************************************************************************/
159
  IPart[PITCH] = controlIntegrals[CONTROL_ELEVATOR] - angle[PITCH];
159
  IPart[PITCH] = error[PITCH]; // * some factor configurable.
160
  if (IPart[PITCH] > PITCHROLLOVER180) IPart[PITCH] -= PITCHROLLOVER360;
-
 
161
  else if (IPart[PITCH] <= -PITCHROLLOVER180) IPart[PITCH] += PITCHROLLOVER360;
-
 
162
  if (IPart[PITCH] > HH_RANGE) IPart[PITCH] = HH_RANGE;
-
 
163
  else if (IPart[PITCH] < -HH_RANGE) IPart[PITCH] = -HH_RANGE;
-
 
164
 
-
 
165
  IPart[ROLL] = controlIntegrals[CONTROL_AILERONS] - angle[ROLL];
-
 
166
  if (IPart[ROLL] > PITCHROLLOVER180) IPart[ROLL] -= PITCHROLLOVER360;
-
 
167
  else if (IPart[ROLL] <= -PITCHROLLOVER180) IPart[ROLL] += PITCHROLLOVER360;
-
 
168
  if (IPart[ROLL] > HH_RANGE) IPart[ROLL] = HH_RANGE;
160
  IPart[ROLL] = error[ROLL];
169
  else if (IPart[ROLL] < -HH_RANGE) IPart[ROLL] = -HH_RANGE;
161
    // TODO: Add ipart. Or add/subtract depending, not sure.
170
 
-
 
171
  term[PITCH] = control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? PDPart[PITCH] : -PDPart[PITCH]);
162
  term[PITCH] = control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? PDPart[PITCH] : -PDPart[PITCH]);
172
  term[ROLL] = control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? PDPart[ROLL] : -PDPart[ROLL]);
163
  term[ROLL] = control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? PDPart[ROLL] : -PDPart[ROLL]);
173
  yawTerm = control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? PDPartYaw : -PDPartYaw);
164
  yawTerm = control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? PDPartYaw : -PDPartYaw);
174
 
-
 
175
 
165
 
176
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
166
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
177
  // Universal Mixer
167
  // Universal Mixer
178
  // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
168
  // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
179
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
169
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
180
 
170
 
181
  DebugOut.Analog[12] = term[PITCH];
171
  DebugOut.Analog[12] = term[PITCH];
182
  DebugOut.Analog[13] = term[ROLL];
172
  DebugOut.Analog[13] = term[ROLL];
183
  DebugOut.Analog[14] = throttleTerm;
173
  DebugOut.Analog[14] = throttleTerm;
184
  DebugOut.Analog[15] = yawTerm;
174
  DebugOut.Analog[15] = yawTerm;
185
 
175
 
186
  for (i = 0; i < MAX_OUTPUTS; i++) {
176
  for (i = 0; i < MAX_OUTPUTS; i++) {
187
    int16_t tmp;
177
    int16_t tmp;
188
      if (outputTestActive) {
178
      if (outputTestActive) {
189
          outputs[i].SetPoint = outputTest[i] * 4;
179
          outputs[i].SetPoint = outputTest[i] * 4;
190
      } else {
180
      } else {
191
        // Follow the normal order of servos: Ailerons, elevator, throttle, rudder.
181
        // Follow the normal order of servos: Ailerons, elevator, throttle, rudder.
192
        switch(i) {
182
        switch(i) {
193
        case 0: tmp = term[ROLL]; break;
183
        case 0: tmp = term[ROLL]; break;
194
        case 1: tmp = term[PITCH]; break;
184
        case 1: tmp = term[PITCH]; break;
195
        case 2: tmp = throttleTerm - 310; break;
185
        case 2: tmp = throttleTerm - 310; break;
196
        case 3: tmp = yawTerm; break;
186
        case 3: tmp = yawTerm; break;
197
        default: tmp = 0;
187
        default: tmp = 0;
198
        }
188
        }
199
      outputFilters[i] = outputFilter(tmp, outputFilters[i]);
189
      outputFilters[i] = outputFilter(tmp, outputFilters[i]);
200
      // Now we scale back down to a 0..255 range.
190
      // Now we scale back down to a 0..255 range.
201
      tmp = outputFilters[i];
191
      tmp = outputFilters[i];
202
      outputs[i].SetPoint = tmp;
192
      outputs[i].SetPoint = tmp;
203
    }
193
    }
204
  }
194
  }
205
 
195
 
206
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
196
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
207
  // Debugging
197
  // Debugging
208
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
198
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
209
  if (!(--debugDataTimer)) {
199
  if (!(--debugDataTimer)) {
210
    debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
200
    debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
211
    DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
201
    DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
212
    DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
202
    DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
213
    DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
203
      DebugOut.Analog[2] = angle[YAW] / GYRO_DEG_FACTOR_YAW;
214
 
204
 
215
    DebugOut.Analog[6] = pitchPFactor;
205
    DebugOut.Analog[6] = pitchPFactor;
216
    DebugOut.Analog[7] = rollPFactor;
206
    DebugOut.Analog[7] = rollPFactor;
217
    DebugOut.Analog[8] = yawPFactor;
207
    DebugOut.Analog[8] = yawPFactor;
218
    DebugOut.Analog[9] = pitchDFactor;
208
    DebugOut.Analog[9] = pitchDFactor;
219
    DebugOut.Analog[10] = rollDFactor;
209
    DebugOut.Analog[10] = rollDFactor;
220
    DebugOut.Analog[11] = yawDFactor;
210
    DebugOut.Analog[11] = yawDFactor;
221
 
211
 
222
    DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
212
    DebugOut.Analog[18] = (10 * error[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
223
    DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
213
    DebugOut.Analog[19] = (10 * error[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
224
    DebugOut.Analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
214
    DebugOut.Analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
225
    DebugOut.Analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
215
    DebugOut.Analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
226
  }
216
  }
227
}
217
}
228
 
218