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