Rev 2087 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2087 | Rev 2088 | ||
---|---|---|---|
1 | #include <stdlib.h> |
1 | #include <stdlib.h> |
2 | #include <avr/io.h> |
2 | #include <avr/io.h> |
3 | 3 | ||
4 | #include "attitude.h" |
4 | #include "attitude.h" |
5 | #include "dongfangMath.h" |
5 | #include "dongfangMath.h" |
6 | #include "commands.h" |
6 | #include "commands.h" |
7 | 7 | ||
8 | // For scope debugging only! |
8 | // For scope debugging only! |
9 | #include "rc.h" |
9 | #include "rc.h" |
10 | 10 | ||
11 | // where our main data flow comes from. |
11 | // where our main data flow comes from. |
12 | #include "analog.h" |
12 | #include "analog.h" |
13 | 13 | ||
14 | #include "configuration.h" |
14 | #include "configuration.h" |
15 | #include "output.h" |
15 | #include "output.h" |
16 | 16 | ||
17 | // Some calculations are performed depending on some stick related things. |
17 | // Some calculations are performed depending on some stick related things. |
18 | #include "controlMixer.h" |
18 | #include "controlMixer.h" |
19 | 19 | ||
20 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
20 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
21 | 21 | ||
22 | /* |
22 | /* |
23 | * Gyro readings, as read from the analog module. It would have been nice to flow |
23 | * Gyro readings, as read from the analog module. It would have been nice to flow |
24 | * them around between the different calculations as a struct or array (doing |
24 | * them around between the different calculations as a struct or array (doing |
25 | * things functionally without side effects) but this is shorter and probably |
25 | * things functionally without side effects) but this is shorter and probably |
26 | * faster too. |
26 | * faster too. |
27 | * The variables are overwritten at each attitude calculation invocation - the values |
27 | * The variables are overwritten at each attitude calculation invocation - the values |
28 | * are not preserved or reused. |
28 | * are not preserved or reused. |
29 | */ |
29 | */ |
30 | int16_t rate_ATT[2], yawRate; |
30 | int16_t rate_ATT[2], yawRate; |
31 | 31 | ||
32 | // With different (less) filtering |
32 | // With different (less) filtering |
33 | int16_t rate_PID[2]; |
33 | int16_t rate_PID[2]; |
34 | int16_t differential[2]; |
34 | int16_t differential[2]; |
35 | 35 | ||
36 | /* |
36 | /* |
37 | * Gyro readings, after performing "axis coupling" - that is, the transfomation |
37 | * Gyro readings, after performing "axis coupling" - that is, the transfomation |
38 | * of rotation rates from the airframe-local coordinate system to a ground-fixed |
38 | * of rotation rates from the airframe-local coordinate system to a ground-fixed |
39 | * coordinate system. If axis copling is disabled, the gyro readings will be |
39 | * coordinate system. If axis copling is disabled, the gyro readings will be |
40 | * copied into these directly. |
40 | * copied into these directly. |
41 | * These are global for the same pragmatic reason as with the gyro readings. |
41 | * These are global for the same pragmatic reason as with the gyro readings. |
42 | * The variables are overwritten at each attitude calculation invocation - the values |
42 | * The variables are overwritten at each attitude calculation invocation - the values |
43 | * are not preserved or reused. |
43 | * are not preserved or reused. |
44 | */ |
44 | */ |
45 | int16_t ACRate[2], ACYawRate; |
45 | int16_t ACRate[2], ACYawRate; |
46 | 46 | ||
47 | /* |
47 | /* |
48 | * Gyro integrals. These are the rotation angles of the airframe compared to the |
48 | * Gyro integrals. These are the rotation angles of the airframe compared to the |
49 | * horizontal plane, yaw relative to yaw at start. |
49 | * horizontal plane, yaw relative to yaw at start. |
50 | */ |
50 | */ |
51 | int32_t attitude[2]; |
51 | int32_t attitude[2]; |
52 | 52 | ||
53 | //int readingHeight = 0; |
53 | //int readingHeight = 0; |
54 | 54 | ||
55 | // Yaw angle and compass stuff. |
55 | // Yaw angle and compass stuff. |
56 | int32_t headingError; |
56 | int32_t headingError; |
57 | 57 | ||
58 | // The difference between the above 2 (heading - course) on a -180..179 degree interval. |
58 | // The difference between the above 2 (heading - course) on a -180..179 degree interval. |
59 | // Not necessary. Never read anywhere. |
59 | // Not necessary. Never read anywhere. |
60 | // int16_t compassOffCourse = 0; |
60 | // int16_t compassOffCourse = 0; |
61 | 61 | ||
62 | uint16_t ignoreCompassTimer = 0;// 500; |
62 | uint16_t ignoreCompassTimer = 0;// 500; |
63 | 63 | ||
64 | int32_t heading; // Yaw Gyro Integral supported by compass |
64 | int32_t heading; // Yaw Gyro Integral supported by compass |
65 | int16_t yawGyroDrift; |
65 | int16_t yawGyroDrift; |
66 | 66 | ||
67 | int16_t correctionSum[2] = { 0, 0 }; |
67 | int16_t correctionSum[2] = { 0, 0 }; |
68 | 68 | ||
69 | // For NaviCTRL use. |
69 | // For NaviCTRL use. |
70 | int16_t averageAcc[2] = { 0, 0 }, averageAccCount = 0; |
70 | int16_t averageAcc[2] = { 0, 0 }, averageAccCount = 0; |
71 | 71 | ||
72 | /* |
72 | /* |
73 | * Experiment: Compensating for dynamic-induced gyro biasing. |
73 | * Experiment: Compensating for dynamic-induced gyro biasing. |
74 | */ |
74 | */ |
75 | int16_t driftComp[2] = { 0, 0 }, driftCompYaw = 0; |
75 | int16_t driftComp[2] = { 0, 0 }, driftCompYaw = 0; |
76 | // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
76 | // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
77 | // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
77 | // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
78 | // int16_t dynamicCalCount; |
78 | // int16_t dynamicCalCount; |
79 | 79 | ||
80 | uint16_t accVector; |
80 | uint16_t accVector; |
81 | 81 | ||
82 | /************************************************************************ |
82 | /************************************************************************ |
83 | * Set inclination angles from the acc. sensor data. |
83 | * Set inclination angles from the acc. sensor data. |
84 | * If acc. sensors are not used, set to zero. |
84 | * If acc. sensors are not used, set to zero. |
85 | * TODO: One could use inverse sine to calculate the angles more |
85 | * TODO: One could use inverse sine to calculate the angles more |
86 | * accurately, but since: 1) the angles are rather small at times when |
86 | * accurately, but since: 1) the angles are rather small at times when |
87 | * it makes sense to set the integrals (standing on ground, or flying at |
87 | * it makes sense to set the integrals (standing on ground, or flying at |
88 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
88 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
89 | * it is hardly worth the trouble. |
89 | * it is hardly worth the trouble. |
90 | ************************************************************************/ |
90 | ************************************************************************/ |
91 | 91 | ||
92 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
92 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
93 | //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
93 | //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
94 | return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; // + correctionTerm; |
94 | return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; // + correctionTerm; |
95 | // return 342L * filteredAcc[axis]; |
95 | // return 342L * filteredAcc[axis]; |
96 | } |
96 | } |
97 | 97 | ||
98 | void setStaticAttitudeAngles(void) { |
98 | void setStaticAttitudeAngles(void) { |
99 | #ifdef ATTITUDE_USE_ACC_SENSORS |
99 | #ifdef ATTITUDE_USE_ACC_SENSORS |
100 | attitude[PITCH] = getAngleEstimateFromAcc(PITCH); |
100 | attitude[PITCH] = getAngleEstimateFromAcc(PITCH); |
101 | attitude[ROLL] = getAngleEstimateFromAcc(ROLL); |
101 | attitude[ROLL] = getAngleEstimateFromAcc(ROLL); |
102 | #else |
102 | #else |
103 | attitude[PITCH] = attitude[ROLL] = 0; |
103 | attitude[PITCH] = attitude[ROLL] = 0; |
104 | #endif |
104 | #endif |
105 | } |
105 | } |
106 | 106 | ||
107 | /************************************************************************ |
107 | /************************************************************************ |
108 | * Neutral Readings |
108 | * Neutral Readings |
109 | ************************************************************************/ |
109 | ************************************************************************/ |
110 | void attitude_setNeutral(void) { |
110 | void attitude_setNeutral(void) { |
111 | // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
111 | // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
112 | // dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0; |
112 | // dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0; |
113 | 113 | ||
114 | driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
114 | driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
115 | correctionSum[PITCH] = correctionSum[ROLL] = 0; |
115 | correctionSum[PITCH] = correctionSum[ROLL] = 0; |
116 | 116 | ||
117 | // Calibrate hardware. |
117 | // Calibrate hardware. |
118 | analog_setNeutral(); |
118 | analog_setNeutral(); |
119 | 119 | ||
120 | // reset gyro integrals to acc guessing |
120 | // reset gyro integrals to acc guessing |
121 | setStaticAttitudeAngles(); |
121 | setStaticAttitudeAngles(); |
122 | #ifdef USE_MK3MAG |
122 | #ifdef USE_MK3MAG |
123 | attitude_resetHeadingToMagnetic(); |
123 | attitude_resetHeadingToMagnetic(); |
124 | #endif |
124 | #endif |
125 | // Servo_On(); //enable servo output |
125 | // Servo_On(); //enable servo output |
126 | } |
126 | } |
127 | 127 | ||
128 | /************************************************************************ |
128 | /************************************************************************ |
129 | * Get sensor data from the analog module, and release the ADC |
129 | * Get sensor data from the analog module, and release the ADC |
130 | * TODO: Ultimately, the analog module could do this (instead of dumping |
130 | * TODO: Ultimately, the analog module could do this (instead of dumping |
131 | * the values into variables). |
131 | * the values into variables). |
132 | * The rate variable end up in a range of about [-1024, 1023]. |
132 | * The rate variable end up in a range of about [-1024, 1023]. |
133 | *************************************************************************/ |
133 | *************************************************************************/ |
134 | void getAnalogData(void) { |
134 | void getAnalogData(void) { |
135 | uint8_t axis; |
135 | uint8_t axis; |
136 | 136 | ||
137 | analog_update(); |
137 | analog_update(); |
138 | 138 | ||
139 | for (axis = PITCH; axis <= ROLL; axis++) { |
139 | for (axis = PITCH; axis <= ROLL; axis++) { |
140 | rate_PID[axis] = gyro_PID[axis] + driftComp[axis]; |
140 | rate_PID[axis] = gyro_PID[axis] + driftComp[axis]; |
141 | rate_ATT[axis] = gyro_ATT[axis] + driftComp[axis]; |
141 | rate_ATT[axis] = gyro_ATT[axis] + driftComp[axis]; |
142 | differential[axis] = gyroD[axis]; |
142 | differential[axis] = gyroD[axis]; |
143 | averageAcc[axis] += acc[axis]; |
143 | averageAcc[axis] += acc[axis]; |
144 | } |
144 | } |
145 | 145 | ||
146 | averageAccCount++; |
146 | averageAccCount++; |
147 | yawRate = yawGyro + driftCompYaw; |
147 | yawRate = yawGyro + driftCompYaw; |
148 | } |
148 | } |
149 | 149 | ||
150 | /* |
150 | /* |
151 | * This is the standard flight-style coordinate system transformation |
151 | * This is the standard flight-style coordinate system transformation |
152 | * (from airframe-local axes to a ground-based system). For some reason |
152 | * (from airframe-local axes to a ground-based system). For some reason |
153 | * the MK uses a left-hand coordinate system. The tranformation has been |
153 | * the MK uses a left-hand coordinate system. The tranformation has been |
154 | * changed accordingly. |
154 | * changed accordingly. |
155 | */ |
155 | */ |
156 | void trigAxisCoupling(void) { |
156 | void trigAxisCoupling(void) { |
157 | int16_t rollAngleInDegrees = attitude[ROLL] / GYRO_DEG_FACTOR_PITCHROLL; |
157 | int16_t rollAngleInDegrees = attitude[ROLL] / GYRO_DEG_FACTOR_PITCHROLL; |
158 | int16_t pitchAngleInDegrees = attitude[PITCH] / GYRO_DEG_FACTOR_PITCHROLL; |
158 | int16_t pitchAngleInDegrees = attitude[PITCH] / GYRO_DEG_FACTOR_PITCHROLL; |
159 | 159 | ||
160 | int16_t cospitch = cos_360(pitchAngleInDegrees); |
160 | int16_t cospitch = cos_360(pitchAngleInDegrees); |
161 | int16_t cosroll = cos_360(rollAngleInDegrees); |
161 | int16_t cosroll = cos_360(rollAngleInDegrees); |
162 | int16_t sinroll = sin_360(rollAngleInDegrees); |
162 | int16_t sinroll = sin_360(rollAngleInDegrees); |
163 | 163 | ||
164 | ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll |
164 | ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll |
165 | - (int32_t) yawRate * sinroll) >> LOG_MATH_UNIT_FACTOR); |
165 | - (int32_t) yawRate * sinroll) >> LOG_MATH_UNIT_FACTOR); |
166 | 166 | ||
167 | ACRate[ROLL] = rate_ATT[ROLL] |
167 | ACRate[ROLL] = rate_ATT[ROLL] |
168 | + (((((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
168 | + (((((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
169 | >> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees)) |
169 | >> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees)) |
170 | >> LOG_MATH_UNIT_FACTOR); |
170 | >> LOG_MATH_UNIT_FACTOR); |
171 | 171 | ||
172 | ACYawRate = |
172 | ACYawRate = |
173 | ((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
173 | ((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
174 | / cospitch; |
174 | / cospitch; |
175 | } |
175 | } |
176 | 176 | ||
177 | // 480 usec with axis coupling - almost no time without. |
177 | // 480 usec with axis coupling - almost no time without. |
178 | void integrate(void) { |
178 | void integrate(void) { |
179 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
179 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
180 | uint8_t axis; |
180 | uint8_t axis; |
181 | 181 | ||
182 | if (staticParams.bitConfig & CFG_AXIS_COUPLING_ENABLED) { |
182 | if (staticParams.bitConfig & CFG_AXIS_COUPLING_ENABLED) { |
183 | trigAxisCoupling(); |
183 | trigAxisCoupling(); |
184 | } else { |
184 | } else { |
185 | ACRate[PITCH] = rate_ATT[PITCH]; |
185 | ACRate[PITCH] = rate_ATT[PITCH]; |
186 | ACRate[ROLL] = rate_ATT[ROLL]; |
186 | ACRate[ROLL] = rate_ATT[ROLL]; |
187 | ACYawRate = yawRate; |
187 | ACYawRate = yawRate; |
188 | } |
188 | } |
189 | 189 | ||
190 | /* |
190 | /* |
191 | * Yaw |
191 | * Yaw |
192 | * Calculate yaw gyro integral (~ to rotation angle) |
192 | * Calculate yaw gyro integral (~ to rotation angle) |
193 | * Limit heading proportional to 0 deg to 360 deg |
193 | * Limit heading proportional to 0 deg to 360 deg |
194 | */ |
194 | */ |
195 | heading += ACYawRate; |
195 | heading += ACYawRate; |
196 | intervalWrap(&heading, YAWOVER360); |
196 | intervalWrap(&heading, YAWOVER360); |
197 | headingError += ACYawRate; |
197 | headingError += ACYawRate; |
198 | 198 | ||
199 | /* |
199 | /* |
200 | * Pitch axis integration and range boundary wrap. |
200 | * Pitch axis integration and range boundary wrap. |
201 | */ |
201 | */ |
202 | for (axis = PITCH; axis <= ROLL; axis++) { |
202 | for (axis = PITCH; axis <= ROLL; axis++) { |
203 | attitude[axis] += ACRate[axis]; |
203 | attitude[axis] += ACRate[axis]; |
204 | if (attitude[axis] > PITCHROLLOVER180) { |
204 | if (attitude[axis] > PITCHROLLOVER180) { |
205 | attitude[axis] -= PITCHROLLOVER360; |
205 | attitude[axis] -= PITCHROLLOVER360; |
206 | } else if (attitude[axis] <= -PITCHROLLOVER180) { |
206 | } else if (attitude[axis] <= -PITCHROLLOVER180) { |
207 | attitude[axis] += PITCHROLLOVER360; |
207 | attitude[axis] += PITCHROLLOVER360; |
208 | } |
208 | } |
209 | } |
209 | } |
210 | } |
210 | } |
211 | 211 | ||
212 | void correctIntegralsByAcc0thOrder_old(void) { |
212 | void correctIntegralsByAcc0thOrder_old(void) { |
213 | uint8_t axis; |
213 | uint8_t axis; |
214 | int32_t temp; |
214 | int32_t temp; |
215 | 215 | ||
216 | uint8_t ca = controlActivity >> 8; |
216 | uint8_t ca = controlActivity >> 8; |
217 | uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
217 | uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
218 | 218 | ||
219 | if (highControlActivity) { |
219 | if (highControlActivity) { |
220 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
220 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
221 | } else { |
221 | } else { |
222 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
222 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
223 | } |
223 | } |
224 | 224 | ||
225 | if (accVector <= staticParams.maxAccVector) { |
225 | if (accVector <= staticParams.maxAccVector) { |
226 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
226 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
227 | 227 | ||
228 | uint8_t permilleAcc = staticParams.zerothOrderCorrection / 8; |
228 | uint8_t permilleAcc = staticParams.zerothOrderCorrection / 8; |
229 | int32_t accDerived; |
229 | int32_t accDerived; |
230 | 230 | ||
231 | /* |
231 | /* |
232 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
232 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
233 | permilleAcc /= 2; |
233 | permilleAcc /= 2; |
234 | debugFullWeight = 0; |
234 | debugFullWeight = 0; |
235 | } |
235 | } |
236 | 236 | ||
237 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
237 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
238 | permilleAcc /= 2; |
238 | permilleAcc /= 2; |
239 | debugFullWeight = 0; |
239 | debugFullWeight = 0; |
240 | */ |
240 | */ |
241 | 241 | ||
242 | if (highControlActivity) { // reduce effect during stick control activity |
242 | if (highControlActivity) { // reduce effect during stick control activity |
243 | permilleAcc /= 4; |
243 | permilleAcc /= 4; |
244 | if (controlActivity > staticParams.maxControlActivity * 2) { // reduce effect during stick control activity |
244 | if (controlActivity > staticParams.maxControlActivity * 2) { // reduce effect during stick control activity |
245 | permilleAcc /= 4; |
245 | permilleAcc /= 4; |
246 | } |
246 | } |
247 | } |
247 | } |
248 | 248 | ||
249 | /* |
249 | /* |
250 | * Add to each sum: The amount by which the angle is changed just below. |
250 | * Add to each sum: The amount by which the angle is changed just below. |
251 | */ |
251 | */ |
252 | for (axis = PITCH; axis <= ROLL; axis++) { |
252 | for (axis = PITCH; axis <= ROLL; axis++) { |
253 | accDerived = getAngleEstimateFromAcc(axis); |
253 | accDerived = getAngleEstimateFromAcc(axis); |
254 | //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
254 | //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
255 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
255 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
256 | temp = attitude[axis]; |
256 | temp = attitude[axis]; |
257 | attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp |
257 | attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp |
258 | + (int32_t) permilleAcc * accDerived) / 1000L; |
258 | + (int32_t) permilleAcc * accDerived) / 1000L; |
259 | correctionSum[axis] += attitude[axis] - temp; |
259 | correctionSum[axis] += attitude[axis] - temp; |
260 | } |
260 | } |
261 | } else { |
261 | } else { |
262 | // experiment: Kill drift compensation updates when not flying smooth. |
262 | // experiment: Kill drift compensation updates when not flying smooth. |
263 | // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
263 | // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
264 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
264 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
265 | } |
265 | } |
266 | } |
266 | } |
267 | 267 | ||
268 | 268 | ||
269 | /************************************************************************ |
269 | /************************************************************************ |
270 | * A kind of 0'th order integral correction, that corrects the integrals |
270 | * A kind of 0'th order integral correction, that corrects the integrals |
271 | * directly. This is the "gyroAccFactor" stuff in the original code. |
271 | * directly. This is the "gyroAccFactor" stuff in the original code. |
272 | * There is (there) also a drift compensation |
272 | * There is (there) also a drift compensation |
273 | * - it corrects the differential of the integral = the gyro offsets. |
273 | * - it corrects the differential of the integral = the gyro offsets. |
274 | * That should only be necessary with drifty gyros like ENC-03. |
274 | * That should only be necessary with drifty gyros like ENC-03. |
275 | ************************************************************************/ |
275 | ************************************************************************/ |
276 | #define LOG_DIVIDER 12 |
276 | #define LOG_DIVIDER 12 |
277 | #define DIVIDER (1L << LOG_DIVIDER) |
277 | #define DIVIDER (1L << LOG_DIVIDER) |
278 | void correctIntegralsByAcc0thOrder_new(void) { |
278 | void correctIntegralsByAcc0thOrder_new(void) { |
279 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
279 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
280 | // are less than ....., or reintroduce Kalman. |
280 | // are less than ....., or reintroduce Kalman. |
281 | // Well actually the Z axis acc. check is not so silly. |
281 | // Well actually the Z axis acc. check is not so silly. |
282 | uint8_t axis; |
282 | uint8_t axis; |
283 | int32_t temp; |
283 | int32_t temp; |
284 | 284 | ||
285 | // for debug LEDs, to be removed with that. |
285 | // for debug LEDs, to be removed with that. |
286 | static uint8_t controlActivityFlash=1; |
286 | static uint8_t controlActivityFlash=1; |
287 | static uint8_t accFlash=1; |
287 | static uint8_t accFlash=1; |
288 | #define CF_MAX 10 |
288 | #define CF_MAX 10 |
289 | // [1..n[=off [n..10]=on |
289 | // [1..n[=off [n..10]=on |
290 | // 1 -->1=on, 2=on, ..., 10=on |
290 | // 1 -->1=on, 2=on, ..., 10=on |
291 | // 2 -->1=off,2=on, ..., 10=on |
291 | // 2 -->1=off,2=on, ..., 10=on |
292 | // 10-->1=off,2=off,..., 10=on |
292 | // 10-->1=off,2=off,..., 10=on |
293 | // 11-->1=off,2=off,..., 10=off |
293 | // 11-->1=off,2=off,..., 10=off |
294 | uint16_t ca = controlActivity >> 6; |
294 | uint16_t ca = controlActivity >> 6; |
295 | uint8_t controlActivityWeighted = ca / staticParams.zerothOrderCorrectionControlTolerance; |
295 | uint8_t controlActivityWeighted = ca / staticParams.zerothOrderCorrectionControlTolerance; |
296 | if (!controlActivityWeighted) controlActivityWeighted = 1; |
296 | if (!controlActivityWeighted) controlActivityWeighted = 1; |
297 | uint8_t accVectorWeighted = accVector / staticParams.zerothOrderCorrectionAccTolerance; |
297 | uint8_t accVectorWeighted = accVector / staticParams.zerothOrderCorrectionAccTolerance; |
298 | if (!accVectorWeighted) accVectorWeighted = 1; |
298 | if (!accVectorWeighted) accVectorWeighted = 1; |
299 | 299 | ||
300 | uint8_t accPart = staticParams.zerothOrderCorrection; |
300 | uint8_t accPart = staticParams.zerothOrderCorrection; |
301 | int32_t accDerived; |
301 | int32_t accDerived; |
302 | 302 | ||
303 | debugOut.analog[14] = controlActivity; |
303 | debugOut.analog[14] = controlActivity; |
304 | debugOut.analog[15] = accVector; |
304 | debugOut.analog[15] = accVector; |
305 | 305 | ||
306 | debugOut.analog[20] = controlActivityWeighted; |
306 | debugOut.analog[20] = controlActivityWeighted; |
307 | debugOut.analog[21] = accVectorWeighted; |
307 | debugOut.analog[21] = accVectorWeighted; |
308 | debugOut.analog[24] = accVector; |
308 | debugOut.analog[24] = accVector; |
309 | 309 | ||
310 | accPart /= controlActivityWeighted; |
310 | accPart /= controlActivityWeighted; |
311 | accPart /= accVectorWeighted; |
311 | accPart /= accVectorWeighted; |
312 | 312 | ||
313 | if (controlActivityFlash < controlActivityWeighted) { |
313 | if (controlActivityFlash < controlActivityWeighted) { |
314 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
314 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
315 | } else { |
315 | } else { |
316 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
316 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
317 | } |
317 | } |
318 | if (++controlActivityFlash > CF_MAX+1) controlActivityFlash=1; |
318 | if (++controlActivityFlash > CF_MAX+1) controlActivityFlash=1; |
319 | 319 | ||
320 | if (accFlash < accVectorWeighted) { |
320 | if (accFlash < accVectorWeighted) { |
321 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
321 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
322 | } else { |
322 | } else { |
323 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
323 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
324 | } |
324 | } |
325 | if (++accFlash > CF_MAX+1) accFlash=1; |
325 | if (++accFlash > CF_MAX+1) accFlash=1; |
326 | 326 | ||
327 | /* |
327 | /* |
328 | * Add to each sum: The amount by which the angle is changed just below. |
328 | * Add to each sum: The amount by which the angle is changed just below. |
329 | */ |
329 | */ |
330 | for (axis = PITCH; axis <= ROLL; axis++) { |
330 | for (axis = PITCH; axis <= ROLL; axis++) { |
331 | accDerived = getAngleEstimateFromAcc(axis); |
331 | accDerived = getAngleEstimateFromAcc(axis); |
332 | //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
332 | //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
333 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
333 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
334 | temp = attitude[axis]; |
334 | temp = attitude[axis]; |
335 | attitude[axis] = ((int32_t) (DIVIDER - accPart) * temp + (int32_t)accPart * accDerived) >> LOG_DIVIDER; |
335 | attitude[axis] = ((int32_t) (DIVIDER - accPart) * temp + (int32_t)accPart * accDerived) >> LOG_DIVIDER; |
336 | correctionSum[axis] += attitude[axis] - temp; |
336 | correctionSum[axis] += attitude[axis] - temp; |
337 | } |
337 | } |
338 | } |
338 | } |
339 | 339 | ||
340 | /************************************************************************ |
340 | /************************************************************************ |
341 | * This is an attempt to correct not the error in the angle integrals |
341 | * This is an attempt to correct not the error in the angle integrals |
342 | * (that happens in correctIntegralsByAcc0thOrder above) but rather the |
342 | * (that happens in correctIntegralsByAcc0thOrder above) but rather the |
343 | * cause of it: Gyro drift, vibration and rounding errors. |
343 | * cause of it: Gyro drift, vibration and rounding errors. |
344 | * All the corrections made in correctIntegralsByAcc0thOrder over |
344 | * All the corrections made in correctIntegralsByAcc0thOrder over |
345 | * DRIFTCORRECTION_TIME cycles are summed up. This number is |
345 | * DRIFTCORRECTION_TIME cycles are summed up. This number is |
346 | * then divided by DRIFTCORRECTION_TIME to get the approx. |
346 | * then divided by DRIFTCORRECTION_TIME to get the approx. |
347 | * correction that should have been applied to each iteration to fix |
347 | * correction that should have been applied to each iteration to fix |
348 | * the error. This is then added to the dynamic offsets. |
348 | * the error. This is then added to the dynamic offsets. |
349 | ************************************************************************/ |
349 | ************************************************************************/ |
350 | // 2 times / sec. = 488/2 |
350 | // 2 times / sec. = 488/2 |
351 | #define DRIFTCORRECTION_TIME 256L |
351 | #define DRIFTCORRECTION_TIME 256L |
352 | void driftCorrection(void) { |
352 | void driftCorrection(void) { |
353 | static int16_t timer = DRIFTCORRECTION_TIME; |
353 | static int16_t timer = DRIFTCORRECTION_TIME; |
354 | int16_t deltaCorrection; |
354 | int16_t deltaCorrection; |
355 | int16_t round; |
355 | int16_t round; |
356 | uint8_t axis; |
356 | uint8_t axis; |
357 | 357 | ||
358 | if (!--timer) { |
358 | if (!--timer) { |
359 | timer = DRIFTCORRECTION_TIME; |
359 | timer = DRIFTCORRECTION_TIME; |
360 | for (axis = PITCH; axis <= ROLL; axis++) { |
360 | for (axis = PITCH; axis <= ROLL; axis++) { |
361 | // Take the sum of corrections applied, add it to delta |
361 | // Take the sum of corrections applied, add it to delta |
362 | if (correctionSum[axis] >= 0) |
362 | if (correctionSum[axis] >= 0) |
363 | round = DRIFTCORRECTION_TIME / 2; |
363 | round = DRIFTCORRECTION_TIME / 2; |
364 | else |
364 | else |
365 | round = -DRIFTCORRECTION_TIME / 2; |
365 | round = -DRIFTCORRECTION_TIME / 2; |
366 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
366 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
367 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
367 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
368 | driftComp[axis] += deltaCorrection / staticParams.driftCompDivider; |
368 | driftComp[axis] += deltaCorrection / staticParams.driftCompDivider; |
369 | CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit); |
369 | CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit); |
370 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
370 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
371 | // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
371 | // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
372 | // debugOut.analog[28 + axis] = driftComp[axis]; |
372 | // debugOut.analog[28 + axis] = driftComp[axis]; |
373 | correctionSum[axis] = 0; |
373 | correctionSum[axis] = 0; |
374 | } |
374 | } |
375 | } |
375 | } |
376 | } |
376 | } |
377 | 377 | ||
378 | void calculateAccVector(void) { |
378 | void calculateAccVector(void) { |
379 | int16_t temp; |
379 | int16_t temp; |
380 | temp = filteredAcc[0] >> 3; |
380 | temp = filteredAcc[0] >> 3; |
381 | accVector = temp * temp; |
381 | accVector = temp * temp; |
382 | temp = filteredAcc[1] >> 3; |
382 | temp = filteredAcc[1] >> 3; |
383 | accVector += temp * temp; |
383 | accVector += temp * temp; |
384 | temp = filteredAcc[2] >> 3; |
384 | temp = filteredAcc[2] >> 3; |
385 | accVector += temp * temp; |
385 | accVector += temp * temp; |
386 | } |
386 | } |
387 | 387 | ||
388 | #ifdef USE_MK3MAG |
388 | #ifdef USE_MK3MAG |
389 | void attitude_resetHeadingToMagnetic(void) { |
389 | void attitude_resetHeadingToMagnetic(void) { |
390 | if (commands_isCalibratingCompass()) |
390 | if (commands_isCalibratingCompass()) |
391 | return; |
391 | return; |
392 | 392 | ||
393 | // Compass is off, skip. |
393 | // Compass is off, skip. |
394 | if (!(staticParams.bitConfig & CFG_COMPASS_ENABLED)) |
394 | if (!(staticParams.bitConfig & CFG_COMPASS_ENABLED)) |
395 | return; |
395 | return; |
396 | 396 | ||
397 | // Compass is invalid, skip. |
397 | // Compass is invalid, skip. |
398 | if (magneticHeading < 0) |
398 | if (magneticHeading < 0) |
399 | return; |
399 | return; |
400 | 400 | ||
401 | heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
401 | heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
402 | //targetHeading = heading; |
402 | //targetHeading = heading; |
403 | headingError = 0; |
403 | headingError = 0; |
404 | } |
404 | } |
405 | 405 | ||
406 | void correctHeadingToMagnetic(void) { |
406 | void correctHeadingToMagnetic(void) { |
407 | int32_t error; |
407 | int32_t error; |
408 | 408 | ||
409 | if (commands_isCalibratingCompass()) { |
409 | if (commands_isCalibratingCompass()) { |
410 | //debugOut.analog[30] = -1; |
410 | //debugOut.analog[30] = -1; |
411 | return; |
411 | return; |
412 | } |
412 | } |
413 | 413 | ||
414 | // Compass is off, skip. |
414 | // Compass is off, skip. |
415 | // Naaah this is assumed. |
415 | // Naaah this is assumed. |
416 | // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
416 | // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
417 | // return; |
417 | // return; |
418 | 418 | ||
419 | // Compass is invalid, skip. |
419 | // Compass is invalid, skip. |
420 | if (magneticHeading < 0) { |
420 | if (magneticHeading < 0) { |
421 | //debugOut.analog[30] = -2; |
421 | //debugOut.analog[30] = -2; |
422 | return; |
422 | return; |
423 | } |
423 | } |
424 | 424 | ||
425 | // Spinning fast, skip |
425 | // Spinning fast, skip |
426 | if (abs(yawRate) > 128) { |
426 | if (abs(yawRate) > 128) { |
427 | // debugOut.analog[30] = -3; |
427 | // debugOut.analog[30] = -3; |
428 | return; |
428 | return; |
429 | } |
429 | } |
430 | 430 | ||
431 | // Otherwise invalidated, skip |
431 | // Otherwise invalidated, skip |
432 | if (ignoreCompassTimer) { |
432 | if (ignoreCompassTimer) { |
433 | ignoreCompassTimer--; |
433 | ignoreCompassTimer--; |
434 | //debugOut.analog[30] = -4; |
434 | //debugOut.analog[30] = -4; |
435 | return; |
435 | return; |
436 | } |
436 | } |
437 | 437 | ||
438 | //debugOut.analog[30] = magneticHeading; |
438 | //debugOut.analog[30] = magneticHeading; |
439 | 439 | ||
440 | // TODO: Find computational cost of this. |
440 | // TODO: Find computational cost of this. |
441 | error = ((int32_t)magneticHeading*GYRO_DEG_FACTOR_YAW - heading); |
441 | error = ((int32_t)magneticHeading*GYRO_DEG_FACTOR_YAW - heading); |
442 | if (error <= -YAWOVER180) error += YAWOVER360; |
442 | if (error <= -YAWOVER180) error += YAWOVER360; |
443 | else if (error > YAWOVER180) error -= YAWOVER360; |
443 | else if (error > YAWOVER180) error -= YAWOVER360; |
444 | 444 | ||
445 | // We only correct errors larger than the resolution of the compass, or else we would keep rounding the |
445 | // We only correct errors larger than the resolution of the compass, or else we would keep rounding the |
446 | // better resolution of the gyros to the worse resolution of the compass all the time. |
446 | // better resolution of the gyros to the worse resolution of the compass all the time. |
447 | // The correction should really only serve to compensate for gyro drift. |
447 | // The correction should really only serve to compensate for gyro drift. |
448 | if(abs(error) < GYRO_DEG_FACTOR_YAW) return; |
448 | if(abs(error) < GYRO_DEG_FACTOR_YAW) return; |
449 | 449 | ||
450 | int32_t correction = (error * staticParams.compassYawCorrection) >> 8; |
450 | int32_t correction = (error * staticParams.compassYawCorrection) >> 8; |
451 | //debugOut.analog[30] = correction; |
451 | //debugOut.analog[30] = correction; |
452 | 452 | ||
453 | debugOut.digital[0] &= ~DEBUG_COMPASS; |
453 | debugOut.digital[0] &= ~DEBUG_COMPASS; |
454 | debugOut.digital[1] &= ~DEBUG_COMPASS; |
454 | debugOut.digital[1] &= ~DEBUG_COMPASS; |
455 | 455 | ||
456 | if (correction > 0) { |
456 | if (correction > 0) { |
457 | debugOut.digital[0] ^= DEBUG_COMPASS; |
457 | debugOut.digital[0] ^= DEBUG_COMPASS; |
458 | } else if (correction < 0) { |
458 | } else if (correction < 0) { |
459 | debugOut.digital[1] ^= DEBUG_COMPASS; |
459 | debugOut.digital[1] ^= DEBUG_COMPASS; |
460 | } |
460 | } |
461 | 461 | ||
462 | // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
462 | // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
463 | // and to the heading error (the angle of yaw that the copter is off the set heading). |
463 | // and to the heading error (the angle of yaw that the copter is off the set heading). |
464 | heading += correction; |
464 | heading += correction; |
465 | headingError += correction; |
465 | headingError += correction; |
466 | intervalWrap(&heading, YAWOVER360); |
466 | intervalWrap(&heading, YAWOVER360); |
467 | 467 | ||
468 | // If we want a transparent flight wrt. compass correction (meaning the copter does not change attitude all |
468 | // If we want a transparent flight wrt. compass correction (meaning the copter does not change attitude all |
469 | // when the compass corrects the heading - it only corrects numbers!) we want to add: |
469 | // when the compass corrects the heading - it only corrects numbers!) we want to add: |
470 | // This will however cause drift to remain uncorrected! |
470 | // This will however cause drift to remain uncorrected! |
471 | // headingError += correction; |
471 | // headingError += correction; |
472 | //debugOut.analog[29] = 0; |
472 | //debugOut.analog[29] = 0; |
473 | } |
473 | } |
474 | #endif |
474 | #endif |
475 | 475 | ||
476 | /************************************************************************ |
476 | /************************************************************************ |
477 | * Main procedure. |
477 | * Main procedure. |
478 | ************************************************************************/ |
478 | ************************************************************************/ |
479 | void calculateFlightAttitude(void) { |
479 | void calculateFlightAttitude(void) { |
480 | getAnalogData(); |
480 | getAnalogData(); |
481 | calculateAccVector(); |
481 | calculateAccVector(); |
482 | integrate(); |
482 | integrate(); |
483 | 483 | ||
484 | #ifdef ATTITUDE_USE_ACC_SENSORS |
484 | #ifdef ATTITUDE_USE_ACC_SENSORS |
485 | if (staticParams.maxControlActivity) { |
485 | if (staticParams.maxControlActivity) { |
486 | correctIntegralsByAcc0thOrder_old(); |
486 | correctIntegralsByAcc0thOrder_old(); |
487 | } else { |
487 | } else { |
488 | correctIntegralsByAcc0thOrder_new(); |
488 | correctIntegralsByAcc0thOrder_new(); |
489 | } |
489 | } |
490 | driftCorrection(); |
490 | driftCorrection(); |
491 | #endif |
491 | #endif |
492 | 492 | ||
493 | // We are done reading variables from the analog module. |
493 | // We are done reading variables from the analog module. |
494 | // Interrupt-driven sensor reading may restart. |
494 | // Interrupt-driven sensor reading may restart. |
495 | startAnalogConversionCycle(); |
495 | startAnalogConversionCycle(); |
496 | 496 | ||
497 | #ifdef USE_MK3MAG |
497 | #ifdef USE_MK3MAG |
498 | if (staticParams.bitConfig & (CFG_COMPASS_ENABLED | CFG_GPS_ENABLED)) { |
498 | if (staticParams.bitConfig & CFG_COMPASS_ENABLED) { |
499 | correctHeadingToMagnetic(); |
499 | correctHeadingToMagnetic(); |
500 | } |
500 | } |
501 | #endif |
501 | #endif |
502 | } |
502 | } |
503 | 503 | ||
504 | /* |
504 | /* |
505 | * This is part of an experiment to measure average sensor offsets caused by motor vibration, |
505 | * This is part of an experiment to measure average sensor offsets caused by motor vibration, |
506 | * and to compensate them away. It brings about some improvement, but no miracles. |
506 | * and to compensate them away. It brings about some improvement, but no miracles. |
507 | * As long as the left stick is kept in the start-motors position, the dynamic compensation |
507 | * As long as the left stick is kept in the start-motors position, the dynamic compensation |
508 | * will measure the effect of vibration, to use for later compensation. So, one should keep |
508 | * will measure the effect of vibration, to use for later compensation. So, one should keep |
509 | * the stick in the start-motors position for a few seconds, till all motors run (at the wrong |
509 | * the stick in the start-motors position for a few seconds, till all motors run (at the wrong |
510 | * speed unfortunately... must find a better way) |
510 | * speed unfortunately... must find a better way) |
511 | */ |
511 | */ |
512 | /* |
512 | /* |
513 | void attitude_startDynamicCalibration(void) { |
513 | void attitude_startDynamicCalibration(void) { |
514 | dynamicCalPitch = dynamicCalRoll = dynamicCalYaw = dynamicCalCount = 0; |
514 | dynamicCalPitch = dynamicCalRoll = dynamicCalYaw = dynamicCalCount = 0; |
515 | savedDynamicOffsetPitch = savedDynamicOffsetRoll = 1000; |
515 | savedDynamicOffsetPitch = savedDynamicOffsetRoll = 1000; |
516 | } |
516 | } |
517 | 517 | ||
518 | void attitude_continueDynamicCalibration(void) { |
518 | void attitude_continueDynamicCalibration(void) { |
519 | // measure dynamic offset now... |
519 | // measure dynamic offset now... |
520 | dynamicCalPitch += hiResPitchGyro; |
520 | dynamicCalPitch += hiResPitchGyro; |
521 | dynamicCalRoll += hiResRollGyro; |
521 | dynamicCalRoll += hiResRollGyro; |
522 | dynamicCalYaw += rawYawGyroSum; |
522 | dynamicCalYaw += rawYawGyroSum; |
523 | dynamicCalCount++; |
523 | dynamicCalCount++; |
524 | 524 | ||
525 | // Param6: Manual mode. The offsets are taken from Param7 and Param8. |
525 | // Param6: Manual mode. The offsets are taken from Param7 and Param8. |
526 | if (dynamicParams.UserParam6 || 1) { // currently always enabled. |
526 | if (dynamicParams.UserParam6 || 1) { // currently always enabled. |
527 | // manual mode |
527 | // manual mode |
528 | driftCompPitch = dynamicParams.UserParam7 - 128; |
528 | driftCompPitch = dynamicParams.UserParam7 - 128; |
529 | driftCompRoll = dynamicParams.UserParam8 - 128; |
529 | driftCompRoll = dynamicParams.UserParam8 - 128; |
530 | } else { |
530 | } else { |
531 | // use the sampled value (does not seem to work so well....) |
531 | // use the sampled value (does not seem to work so well....) |
532 | driftCompPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount; |
532 | driftCompPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount; |
533 | driftCompRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount; |
533 | driftCompRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount; |
534 | driftCompYaw = -dynamicCalYaw / dynamicCalCount; |
534 | driftCompYaw = -dynamicCalYaw / dynamicCalCount; |
535 | } |
535 | } |
536 | 536 | ||
537 | // keep resetting these meanwhile, to avoid accumulating errors. |
537 | // keep resetting these meanwhile, to avoid accumulating errors. |
538 | setStaticAttitudeIntegrals(); |
538 | setStaticAttitudeIntegrals(); |
539 | yawAngle = 0; |
539 | yawAngle = 0; |
540 | } |
540 | } |
541 | */ |
541 | */ |
542 | 542 |