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