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