Rev 2025 | Rev 2102 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2025 | Rev 2099 | ||
---|---|---|---|
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" |
- | |
17 | #ifdef USE_MK3MAG |
- | |
18 | #include "gps.h" |
- | |
19 | #endif |
- | |
20 | 16 | ||
21 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
17 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
22 | 18 | ||
23 | /* |
19 | /* |
24 | * These are no longer maintained, just left at 0. The original implementation just summed the acc. |
20 | * target-directions integrals. |
25 | * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey??? |
- | |
26 | */ |
21 | */ |
27 | // int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
22 | int32_t target[3]; |
- | 23 | ||
28 | 24 | /* |
|
- | 25 | * Error integrals. |
|
29 | int8_t pitchPFactor, rollPFactor, yawPFactor; |
26 | */ |
- | 27 | int32_t error[3]; |
|
- | 28 | ||
- | 29 | uint8_t pFactor[3]; |
|
- | 30 | uint8_t dFactor[3]; |
|
30 | int8_t pitchDFactor, rollDFactor, yawDFactor; |
31 | uint8_t iFactor[3]; |
31 | - | ||
32 | int32_t IPart[2] = {0,0}; |
- | |
33 | - | ||
34 | /************************************************************************/ |
- | |
35 | /* Filter for motor value smoothing (necessary???) */ |
- | |
36 | /************************************************************************/ |
- | |
37 | int16_t outputFilter(int16_t newvalue, int16_t oldvalue) { |
- | |
38 | switch (dynamicParams.UserParams[5]) { |
- | |
39 | case 0: |
- | |
40 | return newvalue; |
- | |
41 | case 1: |
- | |
42 | return (oldvalue + newvalue) / 2; |
- | |
43 | case 2: |
- | |
44 | if (newvalue > oldvalue) |
- | |
45 | return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new |
- | |
46 | else |
32 | uint8_t reverse[3]; |
47 | return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
- | |
48 | case 3: |
- | |
49 | if (newvalue < oldvalue) |
- | |
50 | return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new |
- | |
51 | else |
- | |
52 | return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
- | |
53 | default: |
- | |
54 | return newvalue; |
33 | int32_t IPart[3] = { 0, 0, 0 }; |
55 | } |
34 | |
56 | } |
35 | int16_t servos[MAX_SERVOS]; |
57 | 36 | ||
58 | /************************************************************************/ |
37 | /************************************************************************/ |
59 | /* Neutral Readings */ |
38 | /* Neutral Readings */ |
60 | /************************************************************************/ |
39 | /************************************************************************/ |
61 | #define CONTROL_CONFIG_SCALE 10 |
40 | #define CONTROL_CONFIG_SCALE 10 |
62 | 41 | ||
- | 42 | void flight_setGround(void) { |
|
63 | void flight_setNeutral() { |
43 | IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0; |
- | 44 | target[PITCH] = attitude[PITCH]; |
|
64 | // not really used here any more. |
45 | target[ROLL] = attitude[ROLL]; |
65 | controlMixer_initVariables(); |
46 | target[YAW] = attitude[YAW]; |
66 | } |
47 | } |
67 | - | ||
- | 48 | ||
68 | void setFlightParameters |
49 | void switchToFlightMode(FlightMode_t flightMode) { |
69 | ( |
50 | if (flightMode == FLIGHT_MODE_MANUAL) { |
70 | uint8_t _pitchPFactor, |
51 | pFactor[PITCH] = 0; |
71 | uint8_t _rollPFactor, |
- | |
- | 52 | pFactor[ROLL] = 0; |
|
72 | uint8_t _yawPFactor, |
53 | pFactor[YAW] = 0; |
73 | 54 | } else if (flightMode == FLIGHT_MODE_RATE) { |
|
74 | uint8_t _pitchDFactor, |
55 | pFactor[PITCH] = 0; //staticParams...; |
75 | uint8_t _rollDFactor, |
56 | pFactor[ROLL] = 0; //staticParams...; |
- | 57 | pFactor[YAW] = 0; //staticParams...; |
|
76 | uint8_t _yawDFactor |
58 | } |
77 | ) { |
59 | if (flightMode == FLIGHT_MODE_MANUAL || FLIGHT_MODE_RATE) { |
78 | pitchPFactor = _pitchPFactor; |
60 | iFactor[PITCH] = 0; |
79 | rollPFactor = _rollPFactor; |
- | |
- | 61 | iFactor[ROLL] = 0; |
|
80 | yawPFactor = _yawPFactor; |
62 | iFactor[YAW] = 0; |
81 | 63 | } else if (flightMode == FLIGHT_MODE_ANGLES) { |
|
82 | pitchDFactor = _pitchDFactor; |
64 | iFactor[PITCH] = 0; //staticParams...; |
- | 65 | iFactor[ROLL] = 0; //staticParams...; |
|
- | 66 | iFactor[YAW] = 0; //staticParams...; |
|
- | 67 | // When entering this mode, we want to avoid jerks from accumulated uncorrected errors. |
|
- | 68 | target[PITCH] = attitude[PITCH]; |
|
83 | rollDFactor = _rollDFactor; |
69 | target[ROLL] = attitude[ROLL]; |
84 | yawDFactor = _yawDFactor; |
- | |
85 | } |
- | |
86 | - | ||
87 | void setNormalFlightParameters(void) { |
- | |
88 | setFlightParameters |
- | |
89 | ( |
- | |
90 | dynamicParams.GyroPitchP / CONTROL_CONFIG_SCALE, // 12 seems good |
- | |
91 | dynamicParams.GyroRollP / CONTROL_CONFIG_SCALE, // 9 seems good |
70 | target[YAW] = attitude[YAW]; |
92 | dynamicParams.GyroYawP / (CONTROL_CONFIG_SCALE/2), // 24 seems too little |
71 | } |
93 | 72 | ||
94 | dynamicParams.GyroPitchD / CONTROL_CONFIG_SCALE, |
- | |
95 | dynamicParams.GyroRollD / CONTROL_CONFIG_SCALE, |
- | |
96 | dynamicParams.GyroYawD / CONTROL_CONFIG_SCALE |
73 | dFactor[PITCH] = staticParams.gyroPitchD / CONTROL_CONFIG_SCALE; |
97 | ); |
- | |
98 | } |
74 | dFactor[ROLL] = staticParams.gyroRollD / CONTROL_CONFIG_SCALE; |
99 | 75 | dFactor[YAW] = staticParams.gyroYawD / CONTROL_CONFIG_SCALE; |
|
100 | void setStableFlightParameters(void) { |
76 | |
101 | setFlightParameters(0, 0, 0, 0, 0, 0); |
77 | // TODO: Set reverse also. |
102 | } |
78 | } |
103 | 79 | ||
104 | /************************************************************************/ |
80 | /************************************************************************/ |
105 | /* Main Flight Control */ |
81 | /* Main Flight Control */ |
106 | /************************************************************************/ |
82 | /************************************************************************/ |
107 | void flight_control(void) { |
83 | void flight_control(void) { |
108 | // Mixer Fractions that are combined for Motor Control |
84 | // Mixer Fractions that are combined for Motor Control |
109 | int16_t yawTerm, throttleTerm, term[2]; |
85 | int16_t throttleTerm, term[3]; |
110 | 86 | ||
111 | // PID controller variables |
87 | // PID controller variables |
112 | int16_t PDPart[2], PDPartYaw; |
88 | int16_t PDPart[3]; |
113 | 89 | ||
114 | static int8_t debugDataTimer = 1; |
90 | static int8_t debugDataTimer = 1; |
115 | 91 | ||
116 | // High resolution motor values for smoothing of PID motor outputs |
92 | // High resolution motor values for smoothing of PID motor outputs |
117 | static int16_t outputFilters[MAX_OUTPUTS]; |
93 | // static int16_t outputFilters[MAX_OUTPUTS]; |
118 | 94 | ||
119 | uint8_t i; |
95 | uint8_t axis; |
120 | - | ||
121 | // Fire the main flight attitude calculation, including integration of angles. |
96 | |
- | 97 | // TODO: Check modern version. |
|
122 | // We want that to kick as early as possible, not to delay new AD sampling further. |
98 | // calculateFlightAttitude(); |
123 | calculateFlightAttitude(); |
99 | // TODO: Check modern version. |
124 | controlMixer_update(); |
100 | // controlMixer_update(); |
- | 101 | throttleTerm = controls[CONTROL_THROTTLE]; |
|
125 | throttleTerm = control[CONTROL_THROTTLE]; |
102 | |
126 | 103 | // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
|
127 | /************************************************************************/ |
104 | target[PITCH] += controls[CONTROL_ELEVATOR] * staticParams.stickIElevator; |
128 | /* RC-signal is bad */ |
105 | target[ROLL] += controls[CONTROL_AILERONS] * staticParams.stickIAilerons; |
129 | /************************************************************************/ |
106 | target[YAW] += controls[CONTROL_RUDDER] * staticParams.stickIRudder; |
130 | - | ||
131 | if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not received or noisy |
107 | |
132 | RED_ON; |
108 | for (axis = PITCH; axis <= YAW; axis++) { |
133 | beepRCAlarm(); |
- | |
134 | setStableFlightParameters(); |
109 | if (target[axis] > OVER180) { |
135 | } else { |
110 | target[axis] -= OVER360; |
136 | commands_handleCommands(); |
111 | } else if (attitude[axis] <= -OVER180) { |
137 | setNormalFlightParameters(); |
- | |
138 | } |
- | |
139 | - | ||
140 | /************************************************************************/ |
- | |
141 | /* Calculate control feedback from angle (gyro integral) */ |
- | |
142 | /* and angular velocity (gyro signal) */ |
- | |
143 | /************************************************************************/ |
- | |
144 | PDPart[PITCH] = ((int32_t) rate_PID[PITCH] * pitchPFactor / |
- | |
145 | (256L / CONTROL_SCALING)) |
- | |
146 | + (differential[PITCH] * (int16_t) dynamicParams.GyroPitchD) / 16; |
- | |
147 | - | ||
148 | PDPart[ROLL] = ((int32_t) rate_PID[ROLL] * rollPFactor / |
- | |
149 | (256L / CONTROL_SCALING)) |
- | |
150 | + (differential[ROLL] * (int16_t) dynamicParams.GyroRollD) / 16; |
- | |
151 | - | ||
152 | PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING) |
- | |
153 | + (differential[YAW] * (int16_t) dynamicParams.GyroYawD) / 16; |
- | |
154 | - | ||
155 | /************************************************************************/ |
- | |
156 | /* Stick signals are positive and gyros are negative... */ |
- | |
157 | /************************************************************************/ |
- | |
158 | IPart[PITCH] = error[PITCH]; // * some factor configurable. |
- | |
159 | IPart[ROLL] = error[ROLL]; |
- | |
160 | // TODO: Add ipart. Or add/subtract depending, not sure. |
- | |
161 | term[PITCH] = control[CONTROL_ELEVATOR] + (staticParams.servoDirections & SERVO_DIRECTION_ELEVATOR ? PDPart[PITCH] : -PDPart[PITCH]); |
- | |
- | 112 | attitude[axis] += OVER360; |
|
- | 113 | } |
|
- | 114 | ||
- | 115 | if (reverse[axis]) |
|
- | 116 | error[axis] = attitude[axis] + target[axis]; |
|
- | 117 | else |
|
- | 118 | error[axis] = attitude[axis] - target[axis]; |
|
- | 119 | if (error[axis] > OVER180) { |
|
- | 120 | error[axis] -= OVER360; |
|
- | 121 | } else if (error[axis] <= -OVER180) { |
|
- | 122 | error[axis] += OVER360; |
|
- | 123 | } |
|
162 | term[ROLL] = control[CONTROL_AILERONS] + (staticParams.servoDirections & SERVO_DIRECTION_AILERONS ? PDPart[ROLL] : -PDPart[ROLL]); |
124 | |
- | 125 | /************************************************************************/ |
|
- | 126 | /* Calculate control feedback from angle (gyro integral) */ |
|
- | 127 | /* and angular velocity (gyro signal) */ |
|
- | 128 | /************************************************************************/ |
|
- | 129 | PDPart[axis] = (((int32_t) rate_PID[axis] * pFactor[axis]) >> 6) |
|
- | 130 | + ((differential[axis] * (int16_t) dFactor[axis]) >> 4); |
|
- | 131 | if (reverse[axis]) |
|
163 | yawTerm = control[CONTROL_RUDDER] + (staticParams.servoDirections & SERVO_DIRECTION_RUDDER ? PDPartYaw : -PDPartYaw); |
132 | PDPart[axis] = -PDPart[axis]; |
- | 133 | ||
- | 134 | int16_t anglePart = (error[axis] * iFactor[axis]) >> 10; |
|
- | 135 | if (reverse[axis]) |
|
- | 136 | PDPart[axis] += anglePart; |
|
164 | 137 | else |
|
165 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
138 | PDPart[axis] -= anglePart; |
- | 139 | ||
- | 140 | // Add I parts here... these are integrated errors. |
|
- | 141 | // When an error wraps, actually its I part should be negated or something... |
|
166 | // Universal Mixer |
142 | |
167 | // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
143 | term[axis] = controls[axis] + PDPart[axis] + IPart[axis]; |
168 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
144 | } |
169 | 145 | ||
170 | DebugOut.Analog[12] = term[PITCH]; |
146 | debugOut.analog[12] = term[PITCH]; |
171 | DebugOut.Analog[13] = term[ROLL]; |
147 | debugOut.analog[13] = term[ROLL]; |
172 | DebugOut.Analog[14] = throttleTerm; |
148 | debugOut.analog[14] = throttleTerm; |
173 | DebugOut.Analog[15] = yawTerm; |
149 | debugOut.analog[15] = term[YAW]; |
174 | 150 | ||
175 | for (i = 0; i < MAX_OUTPUTS; i++) { |
151 | for (uint8_t i = 0; i < MAX_SERVOS; i++) { |
176 | int16_t tmp; |
152 | int16_t tmp; |
- | 153 | if (servoTestActive) { |
|
177 | if (outputTestActive) { |
154 | servos[i] = ((int16_t) servoTest[i] - 128) * 4; |
- | 155 | } else { |
|
- | 156 | // Follow the normal order of servos: Ailerons, elevator, throttle, rudder. |
|
178 | outputs[i].SetPoint = outputTest[i] * 4; |
157 | switch (i) { |
- | 158 | case 0: |
|
- | 159 | tmp = term[ROLL]; |
|
179 | } else { |
160 | break; |
- | 161 | case 1: |
|
- | 162 | tmp = term[PITCH]; |
|
180 | // Follow the normal order of servos: Ailerons, elevator, throttle, rudder. |
163 | break; |
- | 164 | case 2: |
|
- | 165 | tmp = throttleTerm; |
|
181 | switch(i) { |
166 | break; |
182 | case 0: tmp = term[ROLL]; break; |
167 | case 3: |
183 | case 1: tmp = term[PITCH]; break; |
- | |
184 | case 2: tmp = throttleTerm - 310; break; |
168 | tmp = term[YAW]; |
185 | case 3: tmp = yawTerm; break; |
- | |
186 | default: tmp = 0; |
169 | break; |
187 | } |
170 | default: |
188 | outputFilters[i] = outputFilter(tmp, outputFilters[i]); |
171 | tmp = 0; |
189 | // Now we scale back down to a 0..255 range. |
172 | } |
190 | tmp = outputFilters[i]; |
173 | // These are all signed and in the same units as the RC stuff in rc.c. |
191 | outputs[i].SetPoint = tmp; |
174 | servos[i] = tmp; |
192 | } |
175 | } |
193 | } |
176 | } |
194 | 177 | ||
195 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
178 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
196 | // Debugging |
179 | // Debugging |
197 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
180 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
198 | if (!(--debugDataTimer)) { |
181 | if (!(--debugDataTimer)) { |
199 | debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
182 | debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
200 | DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
183 | debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
201 | DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
184 | debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
202 | DebugOut.Analog[2] = angle[YAW] / GYRO_DEG_FACTOR_YAW; |
185 | debugOut.analog[2] = attitude[YAW] / (GYRO_DEG_FACTOR / 10); |
203 | 186 | ||
204 | DebugOut.Analog[6] = pitchPFactor; |
187 | debugOut.analog[3] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
205 | DebugOut.Analog[7] = rollPFactor; |
188 | debugOut.analog[4] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
206 | DebugOut.Analog[8] = yawPFactor; |
189 | debugOut.analog[5] = target[YAW] / (GYRO_DEG_FACTOR / 10); |
- | 190 | ||
207 | DebugOut.Analog[9] = pitchDFactor; |
191 | debugOut.analog[6] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
208 | DebugOut.Analog[10] = rollDFactor; |
192 | debugOut.analog[7] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
209 | DebugOut.Analog[11] = yawDFactor; |
193 | debugOut.analog[8] = error[YAW] / (GYRO_DEG_FACTOR / 10); |
210 | 194 | ||
211 | DebugOut.Analog[18] = (10 * error[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
195 | //DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
212 | DebugOut.Analog[19] = (10 * error[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
196 | //DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
213 | DebugOut.Analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
197 | //debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg |
214 | DebugOut.Analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
198 | //debugOut.analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR; // in 0.1 deg |
215 | } |
199 | } |
216 | } |
200 | } |
217 | 201 |