Subversion Repositories FlightCtrl

Rev

Rev 2132 | Rev 2138 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2132 Rev 2135
Line 20... Line 20...
20
int32_t target[3];
20
int32_t target[3];
Line 21... Line 21...
21
 
21
 
22
/*
22
/*
23
 * Error integrals.
23
 * Error integrals.
24
 */
-
 
Line 25... Line 24...
25
int32_t error[3];
24
 */
26
 
25
 
27
uint8_t reverse[3];
26
uint8_t reverse[3];
28
int32_t maxError[3];
27
int32_t maxError[3];
Line 43... Line 42...
43
        target[YAW] = attitude[YAW];
42
        target[YAW] = attitude[YAW];
44
}
43
}
Line 45... Line 44...
45
 
44
 
46
void flight_updateFlightParametersToFlightMode(void) {
45
void flight_updateFlightParametersToFlightMode(void) {
-
 
46
        debugOut.analog[16] = currentFlightMode;
47
        debugOut.analog[16] = currentFlightMode;
47
       
48
        reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR;
48
        reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR;
49
        reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS;
49
        reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS;
Line 50... Line 50...
50
        reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER;
50
        reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER;
Line 128... Line 128...
128
                        target[axis] -= OVER360;
128
                        target[axis] -= OVER360;
129
                } else if (target[axis] <= -OVER180) {
129
                } else if (target[axis] <= -OVER180) {
130
                        target[axis] += OVER360;
130
                        target[axis] += OVER360;
131
                }
131
                }
Line 132... Line 132...
132
 
132
 
Line 133... Line 133...
133
        error[axis] = attitude[axis] - target[axis];
133
                int32_t error = attitude[axis] - target[axis];
134
 
134
 
Line 135... Line 135...
135
#define ROTATETARGET 1
135
#define ROTATETARGET 1
-
 
136
// #define TRUNCATEERROR 1
136
// #define TRUNCATEERROR 1
137
 
137
 
138
#ifdef ROTATETARGET
138
#ifdef ROTATETARGET
139
        //if(abs(error) > OVER180) { // doesnt work!!!
139
                if(abs(error[axis]) > OVER180) {
140
        if(error > OVER180 || error < -OVER180) {
140
                  // The shortest way from attitude to target crosses -180.
141
                  // The shortest way from attitude to target crosses -180.
141
                  // Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative.
142
                  // Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative.
142
                  // Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive.
143
                  // Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive.
143
                  if (error[axis] > 0) {
144
                  if (error > 0) {
144
                    if (error[axis] < OVER360 - maxError[axis]) {
145
                    if (error < OVER360 - maxError[axis]) {
145
                      // too much err.
146
                      // too much err.
146
                      error[axis] = -maxError[axis];
147
                      error = -maxError[axis];
147
                      target[axis] = attitude[axis] + maxError[axis];
148
                      target[axis] = attitude[axis] + maxError[axis];
148
                      if (target[axis] > OVER180) target[axis] -= OVER360;
149
                      if (target[axis] > OVER180) target[axis] -= OVER360;
149
                    } else {
150
                    } else {
150
                      // Normal case, we just need to correct for the wrap. Error will be negative.
151
                      // Normal case, we just need to correct for the wrap. Error will be negative.
151
                      error[axis] -= OVER360;
152
                      error -= OVER360;
152
                    }
153
                    }
153
                  } else {
154
                  } else {
154
            if (error[axis] > maxError[axis] - OVER360) {
155
            if (error > maxError[axis] - OVER360) {
155
              // too much err.
156
              // too much err.
156
              error[axis] = maxError[axis];
157
              error = maxError[axis];
157
              target[axis] = attitude[axis] - maxError[axis];
158
              target[axis] = attitude[axis] - maxError[axis];
158
              if (target[axis] < -OVER180) target[axis] += OVER360;
159
              if (target[axis] < -OVER180) target[axis] += OVER360;
159
            } else {
160
            } else {
160
              // Normal case, we just need to correct for the wrap. Error will be negative.
161
              // Normal case, we just need to correct for the wrap. Error will be negative.
161
              error[axis] += OVER360;
162
              error += OVER360;
162
            }
163
            }
163
                  }
164
                  }
164
                } else {
165
                } else {
165
                  // Simple case, linear range.
166
                  // Simple case, linear range.
166
                if (error[axis] > maxError[axis]) {
167
                if (error > maxError[axis]) {
167
                  error[axis] = maxError[axis];
168
                  error = maxError[axis];
168
                  target[axis] = attitude[axis] - maxError[axis];
169
                  target[axis] = attitude[axis] - maxError[axis];
169
                } else if (error[axis] < -maxError[axis]) {
170
                } else if (error < -maxError[axis]) {
170
              error[axis] = -maxError[axis];
171
              error = -maxError[axis];
171
              target[axis] = attitude[axis] + maxError[axis];
172
              target[axis] = attitude[axis] + maxError[axis];
172
            }
173
            }
173
                }
174
                }
174
#endif
175
#endif
175
#ifdef TUNCATEERROR
176
#ifdef TUNCATEERROR
176
                if (error[axis] > maxError[axis]) {
177
                if (error > maxError[axis]) {
177
                  error[axis] = maxError[axis];
178
                  error = maxError[axis];
178
                } else if (error[axis] < -maxError[axis]) {
179
                } else if (error < -maxError[axis]) {
179
                  error[axis] = -maxError[axis];
180
                  error = -maxError[axis];
180
                } else {
181
                } else {
Line -... Line 182...
-
 
182
                        // update I parts here for angles mode. I parts in rate mode is something different.
-
 
183
                }
181
                        // update I parts here for angles mode. I parts in rate mode is something different.
184
#endif
182
                }
185
 
183
#endif
186
        debugOut.analog[9+axis] = error / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
184
 
187
 
185
                /************************************************************************/
188
                /************************************************************************/
Line 194... Line 197...
194
                } else {
197
                } else {
195
                        PDPart[axis] = 0;
198
                        PDPart[axis] = 0;
196
                }
199
                }
Line 197... Line 200...
197
 
200
 
198
                if (currentFlightMode == FLIGHT_MODE_ANGLES) {
201
                if (currentFlightMode == FLIGHT_MODE_ANGLES) {
199
                        int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE;
202
                        int16_t anglePart = (int32_t)(error * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE;
200
                        PDPart[axis] += anglePart;
203
                        PDPart[axis] += anglePart;
Line 201... Line 204...
201
                }
204
                }
202
 
205
 
Line 256... Line 259...
256
 
259
 
257
                debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
260
                debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
258
                debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
261
                debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
Line 259... Line -...
259
                debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10);
-
 
260
 
-
 
261
                debugOut.analog[9] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
-
 
262
                debugOut.analog[10] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
-
 
263
                debugOut.analog[11] = error[YAW] / (GYRO_DEG_FACTOR / 10);
262
                debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10);
264
 
263
 
265
                debugOut.analog[12] = term[PITCH];
264
                debugOut.analog[12] = term[PITCH];
Line 266... Line 265...
266
                debugOut.analog[13] = term[ROLL];
265
                debugOut.analog[13] = term[ROLL];