Rev 1922 | Rev 2025 | 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 | |||
10 | // for scope debugging |
||
11 | // #include "rc.h" |
||
12 | |||
13 | #include "timer2.h" |
||
14 | #include "attitude.h" |
||
15 | #include "controlMixer.h" |
||
16 | #include "commands.h" |
||
17 | #ifdef USE_MK3MAG |
||
18 | #include "gps.h" |
||
19 | #endif |
||
20 | |||
21 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
||
22 | |||
23 | /* |
||
24 | * These are no longer maintained, just left at 0. The original implementation just summed the acc. |
||
25 | * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey??? |
||
26 | */ |
||
27 | // int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
||
28 | |||
29 | int8_t pitchPFactor, rollPFactor, yawPFactor; |
||
30 | int8_t pitchDFactor, rollDFactor, yawDFactor; |
||
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 |
||
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; |
||
55 | } |
||
56 | } |
||
57 | |||
58 | /************************************************************************/ |
||
59 | /* Neutral Readings */ |
||
60 | /************************************************************************/ |
||
61 | #define CONTROL_CONFIG_SCALE 10 |
||
62 | |||
63 | void flight_setNeutral() { |
||
64 | MKFlags |= MKFLAG_CALIBRATE; |
||
65 | // not really used here any more. |
||
66 | controlMixer_initVariables(); |
||
67 | } |
||
68 | |||
69 | void setFlightParameters |
||
70 | ( |
||
71 | uint8_t _pitchPFactor, |
||
72 | uint8_t _rollPFactor, |
||
73 | uint8_t _yawPFactor, |
||
74 | |||
75 | uint8_t _pitchDFactor, |
||
76 | uint8_t _rollDFactor, |
||
77 | uint8_t _yawDFactor |
||
78 | ) { |
||
79 | pitchPFactor = _pitchPFactor; |
||
80 | rollPFactor = _rollPFactor; |
||
81 | yawPFactor = _yawPFactor; |
||
82 | |||
83 | pitchDFactor = _pitchDFactor; |
||
84 | rollDFactor = _rollDFactor; |
||
85 | yawDFactor = _yawDFactor; |
||
86 | } |
||
87 | |||
88 | void setNormalFlightParameters(void) { |
||
89 | setFlightParameters |
||
90 | ( |
||
91 | dynamicParams.GyroPitchP / CONTROL_CONFIG_SCALE, // 12 seems good |
||
92 | dynamicParams.GyroRollP / CONTROL_CONFIG_SCALE, // 9 seems good |
||
93 | dynamicParams.GyroYawP / (CONTROL_CONFIG_SCALE/2), // 24 seems too little |
||
94 | |||
95 | dynamicParams.GyroPitchD / CONTROL_CONFIG_SCALE, |
||
96 | dynamicParams.GyroRollD / CONTROL_CONFIG_SCALE, |
||
97 | dynamicParams.GyroYawD / CONTROL_CONFIG_SCALE |
||
98 | ); |
||
99 | } |
||
100 | |||
101 | void setStableFlightParameters(void) { |
||
102 | setFlightParameters(0, 0, 0, 0, 0, 0); |
||
103 | } |
||
104 | |||
105 | /************************************************************************/ |
||
106 | /* Main Flight Control */ |
||
107 | /************************************************************************/ |
||
108 | void flight_control(void) { |
||
109 | // Mixer Fractions that are combined for Motor Control |
||
110 | int16_t yawTerm, throttleTerm, term[2]; |
||
111 | |||
112 | // PID controller variables |
||
113 | int16_t PDPart[2], PDPartYaw; |
||
114 | |||
115 | static int8_t debugDataTimer = 1; |
||
116 | |||
117 | // High resolution motor values for smoothing of PID motor outputs |
||
118 | static int16_t outputFilters[MAX_OUTPUTS]; |
||
119 | |||
120 | uint8_t i; |
||
121 | |||
122 | // Fire the main flight attitude calculation, including integration of angles. |
||
123 | // We want that to kick as early as possible, not to delay new AD sampling further. |
||
124 | calculateFlightAttitude(); |
||
125 | controlMixer_update(); |
||
126 | throttleTerm = control[CONTROL_THROTTLE]; |
||
127 | |||
128 | /************************************************************************/ |
||
129 | /* RC-signal is bad */ |
||
130 | /************************************************************************/ |
||
131 | |||
132 | if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not received or noisy |
||
133 | RED_ON; |
||
134 | beepRCAlarm(); |
||
135 | setStableFlightParameters(); |
||
136 | } else { |
||
137 | commands_handleCommands(); |
||
138 | setNormalFlightParameters(); |
||
139 | } |
||
140 | |||
141 | /************************************************************************/ |
||
142 | /* Calculate control feedback from angle (gyro integral) */ |
||
143 | /* and angular velocity (gyro signal) */ |
||
144 | /************************************************************************/ |
||
145 | PDPart[PITCH] = ((int32_t) rate_PID[PITCH] * pitchPFactor / |
||
146 | (256L / CONTROL_SCALING)) |
||
147 | + (differential[PITCH] * (int16_t) dynamicParams.GyroPitchD) / 16; |
||
148 | |||
149 | PDPart[ROLL] = ((int32_t) rate_PID[ROLL] * rollPFactor / |
||
150 | (256L / CONTROL_SCALING)) |
||
151 | + (differential[ROLL] * (int16_t) dynamicParams.GyroRollD) / 16; |
||
152 | |||
153 | PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING) |
||
154 | + (differential[YAW] * (int16_t) dynamicParams.GyroYawD) / 16; |
||
155 | |||
1922 | - | 156 | /************************************************************************/ |
157 | /* Stick signals are positive and gyros are negative... */ |
||
158 | /************************************************************************/ |
||
1927 | - | 159 | IPart[PITCH] = error[PITCH]; // * some factor configurable. |
160 | IPart[ROLL] = error[ROLL]; |
||
161 | // TODO: Add ipart. Or add/subtract depending, not sure. |
||
1910 | - | 162 | term[PITCH] = control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? PDPart[PITCH] : -PDPart[PITCH]); |
163 | term[ROLL] = control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? PDPart[ROLL] : -PDPart[ROLL]); |
||
164 | yawTerm = control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? PDPartYaw : -PDPartYaw); |
||
165 | |||
166 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
167 | // Universal Mixer |
||
168 | // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
||
169 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
170 | |||
171 | DebugOut.Analog[12] = term[PITCH]; |
||
172 | DebugOut.Analog[13] = term[ROLL]; |
||
173 | DebugOut.Analog[14] = throttleTerm; |
||
174 | DebugOut.Analog[15] = yawTerm; |
||
175 | |||
176 | for (i = 0; i < MAX_OUTPUTS; i++) { |
||
177 | int16_t tmp; |
||
178 | if (outputTestActive) { |
||
179 | outputs[i].SetPoint = outputTest[i] * 4; |
||
180 | } else { |
||
181 | // Follow the normal order of servos: Ailerons, elevator, throttle, rudder. |
||
182 | switch(i) { |
||
183 | case 0: tmp = term[ROLL]; break; |
||
184 | case 1: tmp = term[PITCH]; break; |
||
185 | case 2: tmp = throttleTerm - 310; break; |
||
186 | case 3: tmp = yawTerm; break; |
||
187 | default: tmp = 0; |
||
188 | } |
||
189 | outputFilters[i] = outputFilter(tmp, outputFilters[i]); |
||
190 | // Now we scale back down to a 0..255 range. |
||
191 | tmp = outputFilters[i]; |
||
192 | outputs[i].SetPoint = tmp; |
||
193 | } |
||
194 | } |
||
195 | |||
196 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
197 | // Debugging |
||
198 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
199 | if (!(--debugDataTimer)) { |
||
200 | debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
||
201 | DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
||
202 | DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
||
1927 | - | 203 | DebugOut.Analog[2] = angle[YAW] / GYRO_DEG_FACTOR_YAW; |
1910 | - | 204 | |
205 | DebugOut.Analog[6] = pitchPFactor; |
||
206 | DebugOut.Analog[7] = rollPFactor; |
||
207 | DebugOut.Analog[8] = yawPFactor; |
||
208 | DebugOut.Analog[9] = pitchDFactor; |
||
209 | DebugOut.Analog[10] = rollDFactor; |
||
210 | DebugOut.Analog[11] = yawDFactor; |
||
211 | |||
1927 | - | 212 | DebugOut.Analog[18] = (10 * error[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
213 | DebugOut.Analog[19] = (10 * error[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
||
1910 | - | 214 | DebugOut.Analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
215 | DebugOut.Analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
||
216 | } |
||
217 | } |