Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
1910 - 1
#include <stdlib.h>
2
#include <avr/io.h>
3
#include "eeprom.h"
4
#include "flight.h"
5
#include "output.h"
6
 
7
// Necessary for external control and motor test
8
#include "uart0.h"
9
#include "timer2.h"
2103 - 10
#include "analog.h"
1910 - 11
#include "attitude.h"
12
#include "controlMixer.h"
2104 - 13
#include "configuration.h"
1910 - 14
 
15
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
16
 
17
/*
2099 - 18
 * target-directions integrals.
1910 - 19
 */
2099 - 20
int32_t target[3];
1910 - 21
 
2099 - 22
/*
23
 * Error integrals.
24
 */
1910 - 25
 
2099 - 26
uint8_t reverse[3];
2104 - 27
int32_t maxError[3];
2099 - 28
int32_t IPart[3] = { 0, 0, 0 };
2104 - 29
PID_t airspeedPID[3];
1910 - 30
 
2104 - 31
int16_t controlServos[NUM_CONTROL_SERVOS];
1910 - 32
 
33
/************************************************************************/
34
/*  Neutral Readings                                                    */
35
/************************************************************************/
36
#define CONTROL_CONFIG_SCALE 10
37
 
2099 - 38
void flight_setGround(void) {
2102 - 39
        IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0;
40
        target[PITCH] = attitude[PITCH];
41
        target[ROLL] = attitude[ROLL];
42
        target[YAW] = attitude[YAW];
1910 - 43
}
44
 
2104 - 45
void flight_updateFlightParametersToFlightMode(void) {
46
        debugOut.analog[16] = currentFlightMode;
2135 - 47
 
2119 - 48
        reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR;
49
        reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS;
50
        reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER;
1910 - 51
 
2104 - 52
        // At a switch to angles, we want to kill errors first.
53
        // This should be triggered only once per mode change!
54
        if (currentFlightMode == FLIGHT_MODE_ANGLES) {
55
                target[PITCH] = attitude[PITCH];
56
                target[ROLL] = attitude[ROLL];
57
                target[YAW] = attitude[YAW];
58
        }
1910 - 59
 
2104 - 60
        for (uint8_t axis=0; axis<3; axis++) {
61
                maxError[axis] = (int32_t)staticParams.gyroPID[axis].iMax * GYRO_DEG_FACTOR;
62
        }
63
}
2102 - 64
 
2104 - 65
// Normal at airspeed = 10.
66
uint8_t calcAirspeedPID(uint8_t pid) {
2119 - 67
        if (!(staticParams.bitConfig & CFG_USE_AIRSPEED_PID)) {
2104 - 68
                return pid;
2102 - 69
        }
2104 - 70
 
2106 - 71
        uint16_t result = (pid * 10) / airspeedVelocity;
2104 - 72
 
2106 - 73
        if (result > 240 || airspeedVelocity == 0) {
2104 - 74
                result = 240;
75
        }
76
 
77
        return result;
1910 - 78
}
79
 
2104 - 80
void setAirspeedPIDs(void) {
81
        for (uint8_t axis = 0; axis<3; axis++) {
82
                airspeedPID[axis].P = calcAirspeedPID(dynamicParams.gyroPID[axis].P);
83
                airspeedPID[axis].I = calcAirspeedPID(dynamicParams.gyroPID[axis].I); // Should this be???
84
                airspeedPID[axis].D = dynamicParams.gyroPID[axis].D;
85
        }
86
}
87
 
2119 - 88
#define LOG_STICK_SCALE 8
89
#define LOG_P_SCALE 6
90
#define LOG_I_SCALE 10
91
#define LOG_D_SCALE 6
92
 
1910 - 93
/************************************************************************/
94
/*  Main Flight Control                                                 */
95
/************************************************************************/
96
void flight_control(void) {
2102 - 97
        // Mixer Fractions that are combined for Motor Control
2103 - 98
        int16_t term[4];
2099 - 99
 
2102 - 100
        // PID controller variables
101
        int16_t PDPart[3];
1910 - 102
 
2102 - 103
        static int8_t debugDataTimer = 1;
1910 - 104
 
2102 - 105
        // High resolution motor values for smoothing of PID motor outputs
106
        // static int16_t outputFilters[MAX_OUTPUTS];
1910 - 107
 
2102 - 108
        uint8_t axis;
1910 - 109
 
2104 - 110
        setAirspeedPIDs();
111
 
2103 - 112
        term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE];
1910 - 113
 
2102 - 114
        // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.
2122 - 115
        int32_t tmp;
1910 - 116
 
2122 - 117
        tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE;
2126 - 118
        if (reverse[PITCH]) target[PITCH] += tmp; else target[PITCH] -= tmp;
2122 - 119
 
120
        tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE;
2126 - 121
        if (reverse[ROLL]) target[ROLL] += tmp; else target[ROLL] -= tmp;
2122 - 122
 
123
        tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE;
2126 - 124
        if (reverse[YAW]) target[YAW] += tmp; else target[YAW] -= tmp;
2122 - 125
 
2102 - 126
        for (axis = PITCH; axis <= YAW; axis++) {
127
                if (target[axis] > OVER180) {
128
                        target[axis] -= OVER360;
2103 - 129
                } else if (target[axis] <= -OVER180) {
2104 - 130
                        target[axis] += OVER360;
2102 - 131
                }
1910 - 132
 
2135 - 133
                int32_t error = attitude[axis] - target[axis];
2129 - 134
 
2131 - 135
#define ROTATETARGET 1
136
// #define TRUNCATEERROR 1
137
 
2132 - 138
#ifdef ROTATETARGET
2135 - 139
        //if(abs(error) > OVER180) { // doesnt work!!!
140
        if(error > OVER180 || error < -OVER180) {
2131 - 141
                  // The shortest way from attitude to target crosses -180.
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.
143
                  // Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive.
2135 - 144
                  if (error > 0) {
145
                    if (error < OVER360 - maxError[axis]) {
2131 - 146
                      // too much err.
2135 - 147
                      error = -maxError[axis];
2131 - 148
                      target[axis] = attitude[axis] + maxError[axis];
2132 - 149
                      if (target[axis] > OVER180) target[axis] -= OVER360;
2131 - 150
                    } else {
151
                      // Normal case, we just need to correct for the wrap. Error will be negative.
2135 - 152
                      error -= OVER360;
2131 - 153
                    }
154
                  } else {
2135 - 155
            if (error > maxError[axis] - OVER360) {
2131 - 156
              // too much err.
2135 - 157
              error = maxError[axis];
2131 - 158
              target[axis] = attitude[axis] - maxError[axis];
2132 - 159
              if (target[axis] < -OVER180) target[axis] += OVER360;
2131 - 160
            } else {
161
              // Normal case, we just need to correct for the wrap. Error will be negative.
2135 - 162
              error += OVER360;
2131 - 163
            }
164
                  }
165
                } else {
166
                  // Simple case, linear range.
2135 - 167
                if (error > maxError[axis]) {
168
                  error = maxError[axis];
2131 - 169
                  target[axis] = attitude[axis] - maxError[axis];
2135 - 170
                } else if (error < -maxError[axis]) {
171
              error = -maxError[axis];
2131 - 172
              target[axis] = attitude[axis] + maxError[axis];
173
            }
2129 - 174
                }
2131 - 175
#endif
2132 - 176
#ifdef TUNCATEERROR
2135 - 177
                if (error > maxError[axis]) {
178
                  error = maxError[axis];
179
                } else if (error < -maxError[axis]) {
180
                  error = -maxError[axis];
2118 - 181
                } else {
2129 - 182
                        // update I parts here for angles mode. I parts in rate mode is something different.
2102 - 183
                }
2131 - 184
#endif
1910 - 185
 
2135 - 186
        debugOut.analog[9+axis] = error / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
187
 
2102 - 188
                /************************************************************************/
189
                /* Calculate control feedback from angle (gyro integral)                */
190
                /* and angular velocity (gyro signal)                                   */
191
                /************************************************************************/
2119 - 192
                if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode == FLIGHT_MODE_RATE) {
2122 - 193
                        PDPart[axis] = +(((int32_t) gyro_PID[axis] * (int32_t) airspeedPID[axis].P) >> LOG_P_SCALE)
2119 - 194
                                + (((int16_t)gyroD[axis] * (int16_t) airspeedPID[axis].D) >> LOG_D_SCALE);
2122 - 195
                        //if (reverse[axis])
196
                        //      PDPart[axis] = -PDPart[axis];
2104 - 197
                } else {
198
                        PDPart[axis] = 0;
199
                }
1910 - 200
 
2104 - 201
                if (currentFlightMode == FLIGHT_MODE_ANGLES) {
2135 - 202
                        int16_t anglePart = (int32_t)(error * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE;
2122 - 203
                        PDPart[axis] += anglePart;
2104 - 204
                }
2118 - 205
 
2102 - 206
                // Add I parts here... these are integrated errors.
2122 - 207
                if (reverse[axis])
208
                  term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis];
209
                else
210
                  term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis];
2102 - 211
        }
1910 - 212
 
2102 - 213
        debugOut.analog[12] = term[PITCH];
214
        debugOut.analog[13] = term[ROLL];
2103 - 215
        debugOut.analog[14] = term[YAW];
216
        debugOut.analog[15] = term[THROTTLE];
2099 - 217
 
2104 - 218
        for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) {
2102 - 219
                int16_t tmp;
220
                if (servoTestActive) {
2119 - 221
                        controlServos[i] = ((int16_t) servoTest[i] - 128) * 8;
2102 - 222
                } else {
223
                        // Follow the normal order of servos: Ailerons, elevator, throttle, rudder.
224
                        switch (i) {
225
                        case 0:
226
                                tmp = term[ROLL];
227
                                break;
228
                        case 1:
229
                                tmp = term[PITCH];
230
                                break;
231
                        case 2:
2103 - 232
                                tmp = term[THROTTLE];
2102 - 233
                                break;
234
                        case 3:
235
                                tmp = term[YAW];
236
                                break;
237
                        default:
238
                                tmp = 0;
239
                        }
240
                        // These are all signed and in the same units as the RC stuff in rc.c.
241
                        controlServos[i] = tmp;
242
                }
243
        }
1910 - 244
 
2103 - 245
        calculateControlServoValues();
1910 - 246
 
2102 - 247
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
248
        // Debugging
249
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
250
        if (!(--debugDataTimer)) {
251
                debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
2103 - 252
                debugOut.analog[0] = gyro_PID[PITCH]; // in 0.1 deg
253
                debugOut.analog[1] = gyro_PID[ROLL]; // in 0.1 deg
254
                debugOut.analog[2] = gyro_PID[YAW];
1910 - 255
 
2102 - 256
                debugOut.analog[3] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
257
                debugOut.analog[4] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
258
                debugOut.analog[5] = attitude[YAW] / (GYRO_DEG_FACTOR / 10);
2099 - 259
 
2102 - 260
                debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
261
                debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
262
                debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10);
263
 
264
                debugOut.analog[12] = term[PITCH];
265
                debugOut.analog[13] = term[ROLL];
266
                debugOut.analog[14] = term[YAW];
267
        }
1910 - 268
}