Subversion Repositories FlightCtrl

Rev

Rev 2109 | Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
2108 - 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"
10
#include "analog.h"
11
#include "attitude.h"
12
#include "controlMixer.h"
13
#include "configuration.h"
14
 
15
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
16
 
17
/*
18
 * target-directions integrals.
19
 */
20
int32_t target[3];
21
 
22
/*
23
 * Error integrals.
24
 */
25
int32_t error[3];
26
 
27
uint8_t reverse[3];
28
int32_t maxError[3];
29
int32_t IPart[3] = { 0, 0, 0 };
30
PID_t airspeedPID[3];
31
 
32
int16_t controlServos[NUM_CONTROL_SERVOS];
33
 
34
/************************************************************************/
35
/*  Neutral Readings                                                    */
36
/************************************************************************/
37
#define CONTROL_CONFIG_SCALE 10
38
 
39
void flight_setGround(void) {
40
        IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0;
41
        target[PITCH] = attitude[PITCH];
42
        target[ROLL] = attitude[ROLL];
43
        target[YAW] = attitude[YAW];
44
}
45
 
46
void flight_updateFlightParametersToFlightMode(void) {
47
        debugOut.analog[16] = currentFlightMode;
48
        reverse[PITCH] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_ELEVATOR;
49
        reverse[ROLL] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_AILERONS;
50
        reverse[YAW] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_RUDDER;
51
 
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
        }
59
 
60
        for (uint8_t axis=0; axis<3; axis++) {
61
                maxError[axis] = (int32_t)staticParams.gyroPID[axis].iMax * GYRO_DEG_FACTOR;
62
        }
63
}
64
 
65
// Normal at airspeed = 10.
66
uint8_t calcAirspeedPID(uint8_t pid) {
67
        if (staticParams.bitConfig & CFG_USE_AIRSPEED_PID) {
68
                return pid;
69
        }
70
 
71
        uint16_t result = (pid * 10) / airspeedVelocity;
72
 
73
        if (result > 240 || airspeedVelocity == 0) {
74
                result = 240;
75
        }
76
 
77
        return result;
78
}
79
 
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
 
88
/************************************************************************/
89
/*  Main Flight Control                                                 */
90
/************************************************************************/
91
void flight_control(void) {
92
        // Mixer Fractions that are combined for Motor Control
93
        int16_t term[4];
94
 
95
        // PID controller variables
96
        int16_t PDPart[3];
97
 
98
        static int8_t debugDataTimer = 1;
99
 
100
        // High resolution motor values for smoothing of PID motor outputs
101
        // static int16_t outputFilters[MAX_OUTPUTS];
102
 
103
        uint8_t axis;
104
 
105
        setAirspeedPIDs();
106
 
107
        term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE];
108
 
109
        // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.
110
        target[PITCH] += (controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> 6;
111
        target[ROLL] += (controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> 6;
112
        target[YAW] += (controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> 6;
113
 
114
        for (axis = PITCH; axis <= YAW; axis++) {
115
                if (target[axis] > OVER180) {
116
                        target[axis] -= OVER360;
117
                } else if (target[axis] <= -OVER180) {
118
                        target[axis] += OVER360;
119
                }
120
 
121
                if (reverse[axis])
122
                        error[axis] = attitude[axis] + target[axis];
123
                else
124
                        error[axis] = attitude[axis] - target[axis];
125
 
126
                if (error[axis] > maxError[axis]) {
127
                        error[axis] = maxError[axis];
128
                } else if (error[axis] < -maxError[axis]) {
129
                        error[axis] =- maxError[axis];
130
                }
131
 
132
#define LOG_P_SCALE 6
133
#define LOG_I_SCALE 6
134
#define LOG_D_SCALE 4
135
 
136
                /************************************************************************/
137
                /* Calculate control feedback from angle (gyro integral)                */
138
                /* and angular velocity (gyro signal)                                   */
139
                /************************************************************************/
140
                if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode
141
                                == FLIGHT_MODE_RATE) {
142
                        PDPart[axis] = (((int32_t) gyro_PID[axis]
143
                                        * (int16_t) airspeedPID[axis].P) >> LOG_P_SCALE)
144
                                        + ((gyroD[axis] * (int16_t) airspeedPID[axis].D)
145
                                                        >> LOG_D_SCALE);
146
                        if (reverse[axis])
147
                                PDPart[axis] = -PDPart[axis];
148
                } else {
149
                        PDPart[axis] = 0;
150
                }
151
 
152
                if (currentFlightMode == FLIGHT_MODE_ANGLES) {
153
                        int16_t anglePart = (int32_t)(
154
                                        error[axis] * (int32_t) airspeedPID[axis].I)
155
                                        >> LOG_I_SCALE;
156
                        if (reverse[axis])
157
                                PDPart[axis] += anglePart;
158
                        else
159
                                PDPart[axis] -= anglePart;
160
                }
161
                // Add I parts here... these are integrated errors.
162
                // When an error wraps, actually its I part should be negated or something...
163
 
164
                term[axis] = controls[axis] + PDPart[axis] + IPart[axis];
165
        }
166
 
167
        debugOut.analog[12] = term[PITCH];
168
        debugOut.analog[13] = term[ROLL];
169
        debugOut.analog[14] = term[YAW];
170
        debugOut.analog[15] = term[THROTTLE];
171
 
172
        for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) {
173
                int16_t tmp;
174
                if (servoTestActive) {
175
                        controlServos[i] = ((int16_t) servoTest[i] - 128) * 4;
176
                } else {
177
                        // Follow the normal order of servos: Ailerons, elevator, throttle, rudder.
178
                        switch (i) {
179
                        case 0:
180
                                tmp = term[ROLL];
181
                                break;
182
                        case 1:
183
                                tmp = term[PITCH];
184
                                break;
185
                        case 2:
186
                                tmp = term[THROTTLE];
187
                                break;
188
                        case 3:
189
                                tmp = term[YAW];
190
                                break;
191
                        default:
192
                                tmp = 0;
193
                        }
194
                        // These are all signed and in the same units as the RC stuff in rc.c.
195
                        controlServos[i] = tmp;
196
                }
197
        }
198
 
199
        calculateControlServoValues();
200
 
201
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
202
        // Debugging
203
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
204
        if (!(--debugDataTimer)) {
205
                debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
206
                debugOut.analog[0] = gyro_PID[PITCH]; // in 0.1 deg
207
                debugOut.analog[1] = gyro_PID[ROLL]; // in 0.1 deg
208
                debugOut.analog[2] = gyro_PID[YAW];
209
 
210
                debugOut.analog[3] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
211
                debugOut.analog[4] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
212
                debugOut.analog[5] = attitude[YAW] / (GYRO_DEG_FACTOR / 10);
213
 
214
                debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
215
                debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
216
                debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10);
217
 
218
                debugOut.analog[9] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
219
                debugOut.analog[10] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
220
                debugOut.analog[11] = error[YAW] / (GYRO_DEG_FACTOR / 10);
221
 
222
                debugOut.analog[12] = term[PITCH];
223
                debugOut.analog[13] = term[ROLL];
224
                debugOut.analog[14] = term[YAW];
225
 
226
                //DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
227
                //DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
228
                //debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg
229
                //debugOut.analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR; // in 0.1 deg
230
        }
231
}