Rev 2048 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2048 | Rev 2051 | ||
---|---|---|---|
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 | - | ||
57 | // This is updated/written from MM3. Negative angle indicates invalid data. |
- | |
58 | int16_t magneticHeading = -1; |
- | |
59 | - | ||
60 | // This is NOT updated from MM3. Negative angle indicates invalid data. |
- | |
61 | // int16_t headingInDegrees = -1; |
- | |
62 | - | ||
63 | int32_t targetHeading; |
56 | int32_t headingError; |
64 | 57 | ||
65 | // 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. |
66 | // Not necessary. Never read anywhere. |
59 | // Not necessary. Never read anywhere. |
67 | // int16_t compassOffCourse = 0; |
60 | // int16_t compassOffCourse = 0; |
68 | 61 | ||
69 | uint16_t ignoreCompassTimer = 500; |
62 | uint16_t ignoreCompassTimer = 0;// 500; |
70 | 63 | ||
71 | int32_t heading; // Yaw Gyro Integral supported by compass |
64 | int32_t heading; // Yaw Gyro Integral supported by compass |
72 | int16_t yawGyroDrift; |
65 | int16_t yawGyroDrift; |
73 | 66 | ||
74 | int16_t correctionSum[2] = { 0, 0 }; |
67 | int16_t correctionSum[2] = { 0, 0 }; |
75 | 68 | ||
76 | // For NaviCTRL use. |
69 | // For NaviCTRL use. |
77 | int16_t averageAcc[2] = { 0, 0 }, averageAccCount = 0; |
70 | int16_t averageAcc[2] = { 0, 0 }, averageAccCount = 0; |
78 | 71 | ||
79 | /* |
72 | /* |
80 | * Experiment: Compensating for dynamic-induced gyro biasing. |
73 | * Experiment: Compensating for dynamic-induced gyro biasing. |
81 | */ |
74 | */ |
82 | int16_t driftComp[2] = { 0, 0 }, driftCompYaw = 0; |
75 | int16_t driftComp[2] = { 0, 0 }, driftCompYaw = 0; |
83 | // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
76 | // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
84 | // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
77 | // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
85 | // int16_t dynamicCalCount; |
78 | // int16_t dynamicCalCount; |
86 | 79 | ||
87 | uint16_t accVector; |
80 | uint16_t accVector; |
88 | 81 | ||
89 | /************************************************************************ |
82 | /************************************************************************ |
90 | * Set inclination angles from the acc. sensor data. |
83 | * Set inclination angles from the acc. sensor data. |
91 | * If acc. sensors are not used, set to zero. |
84 | * If acc. sensors are not used, set to zero. |
92 | * TODO: One could use inverse sine to calculate the angles more |
85 | * TODO: One could use inverse sine to calculate the angles more |
93 | * 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 |
94 | * 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 |
95 | * 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, |
96 | * it is hardly worth the trouble. |
89 | * it is hardly worth the trouble. |
97 | ************************************************************************/ |
90 | ************************************************************************/ |
98 | 91 | ||
99 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
92 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
100 | //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
93 | //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
101 | return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; // + correctionTerm; |
94 | return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; // + correctionTerm; |
102 | // return 342L * filteredAcc[axis]; |
95 | // return 342L * filteredAcc[axis]; |
103 | } |
96 | } |
104 | 97 | ||
105 | void setStaticAttitudeAngles(void) { |
98 | void setStaticAttitudeAngles(void) { |
106 | #ifdef ATTITUDE_USE_ACC_SENSORS |
99 | #ifdef ATTITUDE_USE_ACC_SENSORS |
107 | attitude[PITCH] = getAngleEstimateFromAcc(PITCH); |
100 | attitude[PITCH] = getAngleEstimateFromAcc(PITCH); |
108 | attitude[ROLL] = getAngleEstimateFromAcc(ROLL); |
101 | attitude[ROLL] = getAngleEstimateFromAcc(ROLL); |
109 | #else |
102 | #else |
110 | attitude[PITCH] = attitude[ROLL] = 0; |
103 | attitude[PITCH] = attitude[ROLL] = 0; |
111 | #endif |
104 | #endif |
112 | } |
105 | } |
113 | 106 | ||
114 | /************************************************************************ |
107 | /************************************************************************ |
115 | * Neutral Readings |
108 | * Neutral Readings |
116 | ************************************************************************/ |
109 | ************************************************************************/ |
117 | void attitude_setNeutral(void) { |
110 | void attitude_setNeutral(void) { |
118 | // 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. |
119 | // dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0; |
112 | // dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0; |
120 | 113 | ||
121 | driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
114 | driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
122 | correctionSum[PITCH] = correctionSum[ROLL] = 0; |
115 | correctionSum[PITCH] = correctionSum[ROLL] = 0; |
123 | 116 | ||
124 | // Calibrate hardware. |
117 | // Calibrate hardware. |
125 | analog_setNeutral(); |
118 | analog_setNeutral(); |
126 | 119 | ||
127 | // reset gyro integrals to acc guessing |
120 | // reset gyro integrals to acc guessing |
128 | setStaticAttitudeAngles(); |
121 | setStaticAttitudeAngles(); |
129 | attitude_resetHeadingToMagnetic(); |
122 | attitude_resetHeadingToMagnetic(); |
130 | // Servo_On(); //enable servo output |
123 | // Servo_On(); //enable servo output |
131 | } |
124 | } |
132 | 125 | ||
133 | /************************************************************************ |
126 | /************************************************************************ |
134 | * Get sensor data from the analog module, and release the ADC |
127 | * Get sensor data from the analog module, and release the ADC |
135 | * TODO: Ultimately, the analog module could do this (instead of dumping |
128 | * TODO: Ultimately, the analog module could do this (instead of dumping |
136 | * the values into variables). |
129 | * the values into variables). |
137 | * The rate variable end up in a range of about [-1024, 1023]. |
130 | * The rate variable end up in a range of about [-1024, 1023]. |
138 | *************************************************************************/ |
131 | *************************************************************************/ |
139 | void getAnalogData(void) { |
132 | void getAnalogData(void) { |
140 | uint8_t axis; |
133 | uint8_t axis; |
141 | 134 | ||
142 | analog_update(); |
135 | analog_update(); |
143 | 136 | ||
144 | for (axis = PITCH; axis <= ROLL; axis++) { |
137 | for (axis = PITCH; axis <= ROLL; axis++) { |
145 | rate_PID[axis] = gyro_PID[axis] + driftComp[axis]; |
138 | rate_PID[axis] = gyro_PID[axis] + driftComp[axis]; |
146 | rate_ATT[axis] = gyro_ATT[axis] + driftComp[axis]; |
139 | rate_ATT[axis] = gyro_ATT[axis] + driftComp[axis]; |
147 | differential[axis] = gyroD[axis]; |
140 | differential[axis] = gyroD[axis]; |
148 | averageAcc[axis] += acc[axis]; |
141 | averageAcc[axis] += acc[axis]; |
149 | } |
142 | } |
150 | 143 | ||
151 | averageAccCount++; |
144 | averageAccCount++; |
152 | yawRate = yawGyro + driftCompYaw; |
145 | yawRate = yawGyro + driftCompYaw; |
153 | } |
146 | } |
154 | 147 | ||
155 | /* |
148 | /* |
156 | * This is the standard flight-style coordinate system transformation |
149 | * This is the standard flight-style coordinate system transformation |
157 | * (from airframe-local axes to a ground-based system). For some reason |
150 | * (from airframe-local axes to a ground-based system). For some reason |
158 | * the MK uses a left-hand coordinate system. The tranformation has been |
151 | * the MK uses a left-hand coordinate system. The tranformation has been |
159 | * changed accordingly. |
152 | * changed accordingly. |
160 | */ |
153 | */ |
161 | void trigAxisCoupling(void) { |
154 | void trigAxisCoupling(void) { |
162 | int16_t rollAngleInDegrees = attitude[ROLL] / GYRO_DEG_FACTOR_PITCHROLL; |
155 | int16_t rollAngleInDegrees = attitude[ROLL] / GYRO_DEG_FACTOR_PITCHROLL; |
163 | int16_t pitchAngleInDegrees = attitude[PITCH] / GYRO_DEG_FACTOR_PITCHROLL; |
156 | int16_t pitchAngleInDegrees = attitude[PITCH] / GYRO_DEG_FACTOR_PITCHROLL; |
164 | 157 | ||
165 | int16_t cospitch = cos_360(pitchAngleInDegrees); |
158 | int16_t cospitch = cos_360(pitchAngleInDegrees); |
166 | int16_t cosroll = cos_360(rollAngleInDegrees); |
159 | int16_t cosroll = cos_360(rollAngleInDegrees); |
167 | int16_t sinroll = sin_360(rollAngleInDegrees); |
160 | int16_t sinroll = sin_360(rollAngleInDegrees); |
168 | 161 | ||
169 | ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll |
162 | ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll |
170 | - (int32_t) yawRate * sinroll) >> LOG_MATH_UNIT_FACTOR); |
163 | - (int32_t) yawRate * sinroll) >> LOG_MATH_UNIT_FACTOR); |
171 | 164 | ||
172 | ACRate[ROLL] = rate_ATT[ROLL] |
165 | ACRate[ROLL] = rate_ATT[ROLL] |
173 | + (((((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
166 | + (((((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
174 | >> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees)) |
167 | >> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees)) |
175 | >> LOG_MATH_UNIT_FACTOR); |
168 | >> LOG_MATH_UNIT_FACTOR); |
176 | 169 | ||
177 | ACYawRate = |
170 | ACYawRate = |
178 | ((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
171 | ((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
179 | / cospitch; |
172 | / cospitch; |
180 | 173 | ||
181 | ACYawRate = |
174 | ACYawRate = |
182 | ((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
175 | ((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
183 | / cospitch; |
176 | / cospitch; |
184 | } |
177 | } |
185 | 178 | ||
186 | // 480 usec with axis coupling - almost no time without. |
179 | // 480 usec with axis coupling - almost no time without. |
187 | void integrate(void) { |
180 | void integrate(void) { |
188 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
181 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
189 | uint8_t axis; |
182 | uint8_t axis; |
190 | 183 | ||
191 | if (staticParams.bitConfig & CFG_AXIS_COUPLING_ACTIVE) { |
184 | if (staticParams.bitConfig & CFG_AXIS_COUPLING_ACTIVE) { |
192 | trigAxisCoupling(); |
185 | trigAxisCoupling(); |
193 | } else { |
186 | } else { |
194 | ACRate[PITCH] = rate_ATT[PITCH]; |
187 | ACRate[PITCH] = rate_ATT[PITCH]; |
195 | ACRate[ROLL] = rate_ATT[ROLL]; |
188 | ACRate[ROLL] = rate_ATT[ROLL]; |
196 | ACYawRate = yawRate; |
189 | ACYawRate = yawRate; |
197 | } |
190 | } |
198 | 191 | ||
199 | /* |
192 | /* |
200 | * Yaw |
193 | * Yaw |
201 | * Calculate yaw gyro integral (~ to rotation angle) |
194 | * Calculate yaw gyro integral (~ to rotation angle) |
202 | * Limit heading proportional to 0 deg to 360 deg |
195 | * Limit heading proportional to 0 deg to 360 deg |
203 | */ |
196 | */ |
204 | heading += ACYawRate; |
197 | heading += ACYawRate; |
205 | intervalWrap(&heading, YAWOVER360); |
198 | intervalWrap(&heading, YAWOVER360); |
- | 199 | ||
- | 200 | headingError += ACYawRate; |
|
- | 201 | ||
- | 202 | debugOut.analog[27] = heading / 100; |
|
- | 203 | ||
206 | /* |
204 | /* |
207 | * Pitch axis integration and range boundary wrap. |
205 | * Pitch axis integration and range boundary wrap. |
208 | */ |
206 | */ |
209 | for (axis = PITCH; axis <= ROLL; axis++) { |
207 | for (axis = PITCH; axis <= ROLL; axis++) { |
210 | attitude[axis] += ACRate[axis]; |
208 | attitude[axis] += ACRate[axis]; |
211 | if (attitude[axis] > PITCHROLLOVER180) { |
209 | if (attitude[axis] > PITCHROLLOVER180) { |
212 | attitude[axis] -= PITCHROLLOVER360; |
210 | attitude[axis] -= PITCHROLLOVER360; |
213 | } else if (attitude[axis] <= -PITCHROLLOVER180) { |
211 | } else if (attitude[axis] <= -PITCHROLLOVER180) { |
214 | attitude[axis] += PITCHROLLOVER360; |
212 | attitude[axis] += PITCHROLLOVER360; |
215 | } |
213 | } |
216 | } |
214 | } |
217 | } |
215 | } |
218 | 216 | ||
219 | /************************************************************************ |
217 | /************************************************************************ |
220 | * A kind of 0'th order integral correction, that corrects the integrals |
218 | * A kind of 0'th order integral correction, that corrects the integrals |
221 | * directly. This is the "gyroAccFactor" stuff in the original code. |
219 | * directly. This is the "gyroAccFactor" stuff in the original code. |
222 | * There is (there) also a drift compensation |
220 | * There is (there) also a drift compensation |
223 | * - it corrects the differential of the integral = the gyro offsets. |
221 | * - it corrects the differential of the integral = the gyro offsets. |
224 | * That should only be necessary with drifty gyros like ENC-03. |
222 | * That should only be necessary with drifty gyros like ENC-03. |
225 | ************************************************************************/ |
223 | ************************************************************************/ |
226 | void correctIntegralsByAcc0thOrder(void) { |
224 | void correctIntegralsByAcc0thOrder(void) { |
227 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
225 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
228 | // are less than ....., or reintroduce Kalman. |
226 | // are less than ....., or reintroduce Kalman. |
229 | // Well actually the Z axis acc. check is not so silly. |
227 | // Well actually the Z axis acc. check is not so silly. |
230 | uint8_t axis; |
228 | uint8_t axis; |
231 | int32_t temp; |
229 | int32_t temp; |
232 | 230 | ||
233 | uint8_t ca = controlActivity >> 8; |
231 | uint8_t ca = controlActivity >> 8; |
234 | uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
232 | uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
235 | 233 | ||
236 | if (highControlActivity) { |
234 | if (highControlActivity) { |
237 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
235 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
238 | } else { |
236 | } else { |
239 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
237 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
240 | } |
238 | } |
241 | 239 | ||
242 | if (accVector <= dynamicParams.maxAccVector) { |
240 | if (accVector <= dynamicParams.maxAccVector) { |
243 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
241 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
244 | 242 | ||
245 | uint8_t permilleAcc = staticParams.zerothOrderCorrection; |
243 | uint8_t permilleAcc = staticParams.zerothOrderCorrection; |
246 | int32_t accDerived; |
244 | int32_t accDerived; |
247 | 245 | ||
248 | /* |
246 | /* |
249 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
247 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
250 | permilleAcc /= 2; |
248 | permilleAcc /= 2; |
251 | debugFullWeight = 0; |
249 | debugFullWeight = 0; |
252 | } |
250 | } |
253 | 251 | ||
254 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
252 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
255 | permilleAcc /= 2; |
253 | permilleAcc /= 2; |
256 | debugFullWeight = 0; |
254 | debugFullWeight = 0; |
257 | */ |
255 | */ |
258 | 256 | ||
259 | if (highControlActivity) { // reduce effect during stick control activity |
257 | if (highControlActivity) { // reduce effect during stick control activity |
260 | permilleAcc /= 4; |
258 | permilleAcc /= 4; |
261 | if (controlActivity > staticParams.maxControlActivity * 2) { // reduce effect during stick control activity |
259 | if (controlActivity > staticParams.maxControlActivity * 2) { // reduce effect during stick control activity |
262 | permilleAcc /= 4; |
260 | permilleAcc /= 4; |
263 | } |
261 | } |
264 | } |
262 | } |
265 | 263 | ||
266 | /* |
264 | /* |
267 | * Add to each sum: The amount by which the angle is changed just below. |
265 | * Add to each sum: The amount by which the angle is changed just below. |
268 | */ |
266 | */ |
269 | for (axis = PITCH; axis <= ROLL; axis++) { |
267 | for (axis = PITCH; axis <= ROLL; axis++) { |
270 | accDerived = getAngleEstimateFromAcc(axis); |
268 | accDerived = getAngleEstimateFromAcc(axis); |
271 | debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
269 | debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
272 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
270 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
273 | temp = attitude[axis]; |
271 | temp = attitude[axis]; |
274 | attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp |
272 | attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp |
275 | + (int32_t) permilleAcc * accDerived) / 1000L; |
273 | + (int32_t) permilleAcc * accDerived) / 1000L; |
276 | correctionSum[axis] += attitude[axis] - temp; |
274 | correctionSum[axis] += attitude[axis] - temp; |
277 | } |
275 | } |
278 | } else { |
276 | } else { |
279 | debugOut.analog[9] = 0; |
277 | debugOut.analog[9] = 0; |
280 | debugOut.analog[10] = 0; |
278 | debugOut.analog[10] = 0; |
281 | // experiment: Kill drift compensation updates when not flying smooth. |
279 | // experiment: Kill drift compensation updates when not flying smooth. |
282 | // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
280 | // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
283 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
281 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
284 | } |
282 | } |
285 | } |
283 | } |
286 | 284 | ||
287 | /************************************************************************ |
285 | /************************************************************************ |
288 | * This is an attempt to correct not the error in the angle integrals |
286 | * This is an attempt to correct not the error in the angle integrals |
289 | * (that happens in correctIntegralsByAcc0thOrder above) but rather the |
287 | * (that happens in correctIntegralsByAcc0thOrder above) but rather the |
290 | * cause of it: Gyro drift, vibration and rounding errors. |
288 | * cause of it: Gyro drift, vibration and rounding errors. |
291 | * All the corrections made in correctIntegralsByAcc0thOrder over |
289 | * All the corrections made in correctIntegralsByAcc0thOrder over |
292 | * DRIFTCORRECTION_TIME cycles are summed up. This number is |
290 | * DRIFTCORRECTION_TIME cycles are summed up. This number is |
293 | * then divided by DRIFTCORRECTION_TIME to get the approx. |
291 | * then divided by DRIFTCORRECTION_TIME to get the approx. |
294 | * correction that should have been applied to each iteration to fix |
292 | * correction that should have been applied to each iteration to fix |
295 | * the error. This is then added to the dynamic offsets. |
293 | * the error. This is then added to the dynamic offsets. |
296 | ************************************************************************/ |
294 | ************************************************************************/ |
297 | // 2 times / sec. = 488/2 |
295 | // 2 times / sec. = 488/2 |
298 | #define DRIFTCORRECTION_TIME 256L |
296 | #define DRIFTCORRECTION_TIME 256L |
299 | void driftCorrection(void) { |
297 | void driftCorrection(void) { |
300 | static int16_t timer = DRIFTCORRECTION_TIME; |
298 | static int16_t timer = DRIFTCORRECTION_TIME; |
301 | int16_t deltaCorrection; |
299 | int16_t deltaCorrection; |
302 | int16_t round; |
300 | int16_t round; |
303 | uint8_t axis; |
301 | uint8_t axis; |
304 | 302 | ||
305 | if (!--timer) { |
303 | if (!--timer) { |
306 | timer = DRIFTCORRECTION_TIME; |
304 | timer = DRIFTCORRECTION_TIME; |
307 | for (axis = PITCH; axis <= ROLL; axis++) { |
305 | for (axis = PITCH; axis <= ROLL; axis++) { |
308 | // Take the sum of corrections applied, add it to delta |
306 | // Take the sum of corrections applied, add it to delta |
309 | if (correctionSum[axis] >= 0) |
307 | if (correctionSum[axis] >= 0) |
310 | round = DRIFTCORRECTION_TIME / 2; |
308 | round = DRIFTCORRECTION_TIME / 2; |
311 | else |
309 | else |
312 | round = -DRIFTCORRECTION_TIME / 2; |
310 | round = -DRIFTCORRECTION_TIME / 2; |
313 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
311 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
314 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
312 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
315 | driftComp[axis] += deltaCorrection / staticParams.driftCompDivider; |
313 | driftComp[axis] += deltaCorrection / staticParams.driftCompDivider; |
316 | CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit); |
314 | CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit); |
317 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
315 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
318 | // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
316 | // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
319 | // debugOut.analog[28 + axis] = driftComp[axis]; |
317 | // debugOut.analog[28 + axis] = driftComp[axis]; |
320 | correctionSum[axis] = 0; |
318 | correctionSum[axis] = 0; |
321 | } |
319 | } |
322 | } |
320 | } |
323 | } |
321 | } |
324 | 322 | ||
325 | void calculateAccVector(void) { |
323 | void calculateAccVector(void) { |
326 | int16_t temp; |
324 | int16_t temp; |
327 | temp = filteredAcc[0] >> 3; |
325 | temp = filteredAcc[0] >> 3; |
328 | accVector = temp * temp; |
326 | accVector = temp * temp; |
329 | temp = filteredAcc[1] >> 3; |
327 | temp = filteredAcc[1] >> 3; |
330 | accVector += temp * temp; |
328 | accVector += temp * temp; |
331 | temp = filteredAcc[2] >> 3; |
329 | temp = filteredAcc[2] >> 3; |
332 | accVector += temp * temp; |
330 | accVector += temp * temp; |
333 | //debugOut.analog[18] = accVector; |
331 | //debugOut.analog[18] = accVector; |
334 | } |
332 | } |
335 | 333 | ||
336 | void attitude_resetHeadingToMagnetic(void) { |
334 | void attitude_resetHeadingToMagnetic(void) { |
337 | if (commands_isCalibratingCompass()) |
335 | if (commands_isCalibratingCompass()) |
338 | return; |
336 | return; |
339 | 337 | ||
340 | // Compass is off, skip. |
338 | // Compass is off, skip. |
341 | if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
339 | if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
342 | return; |
340 | return; |
343 | 341 | ||
344 | // Compass is invalid, skip. |
342 | // Compass is invalid, skip. |
345 | if (magneticHeading < 0) |
343 | if (magneticHeading < 0) |
346 | return; |
344 | return; |
347 | 345 | ||
348 | heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
346 | heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
349 | targetHeading = heading; |
347 | //targetHeading = heading; |
- | 348 | headingError = 0; |
|
350 | 349 | ||
351 | debugOut.digital[0] ^= DEBUG_COMPASS; |
350 | debugOut.digital[0] ^= DEBUG_COMPASS; |
352 | } |
351 | } |
353 | 352 | ||
354 | void correctHeadingToMagnetic(void) { |
353 | void correctHeadingToMagnetic(void) { |
355 | int32_t error; |
354 | int32_t error; |
356 | - | ||
357 | debugOut.analog[27] = heading; |
- | |
358 | 355 | ||
- | 356 | if (commands_isCalibratingCompass()) { |
|
359 | if (commands_isCalibratingCompass()) |
357 | debugOut.analog[29] = 1; |
- | 358 | return; |
|
360 | return; |
359 | } |
361 | 360 | ||
362 | // Compass is off, skip. |
361 | // Compass is off, skip. |
363 | // Naaah this is assumed. |
362 | // Naaah this is assumed. |
364 | // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
363 | // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
365 | // return; |
364 | // return; |
366 | 365 | ||
367 | // Compass is invalid, skip. |
366 | // Compass is invalid, skip. |
368 | if (magneticHeading < 0) |
367 | if (magneticHeading < 0) { |
- | 368 | debugOut.analog[29] = 2; |
|
369 | return; |
369 | return; |
- | 370 | } |
|
370 | 371 | ||
371 | // Spinning fast, skip |
372 | // Spinning fast, skip |
- | 373 | if (abs(yawRate) > 128) { |
|
372 | if (abs(yawRate) > 128) |
374 | debugOut.analog[29] = 3; |
- | 375 | return; |
|
373 | return; |
376 | } |
374 | 377 | ||
375 | // Otherwise invalidated, skip |
378 | // Otherwise invalidated, skip |
376 | if (ignoreCompassTimer) { |
379 | if (ignoreCompassTimer) { |
377 | ignoreCompassTimer--; |
380 | ignoreCompassTimer--; |
- | 381 | debugOut.analog[29] = 4; |
|
378 | return; |
382 | return; |
379 | } |
383 | } |
380 | 384 | ||
381 | // TODO: Find computational cost of this. |
385 | // TODO: Find computational cost of this. |
382 | error = (magneticHeading*GYRO_DEG_FACTOR_YAW - heading) % GYRO_DEG_FACTOR_YAW; |
386 | error = ((int32_t)magneticHeading*GYRO_DEG_FACTOR_YAW - heading); |
- | 387 | if (error <= -YAWOVER180) error += YAWOVER360; |
|
- | 388 | else if (error > YAWOVER180) error -= YAWOVER360; |
|
383 | 389 | ||
384 | // We only correct errors larger than the resolution of the compass, or else we would keep rounding the |
390 | // We only correct errors larger than the resolution of the compass, or else we would keep rounding the |
385 | // better resolution of the gyros to the worse resolution of the compass all the time. |
391 | // better resolution of the gyros to the worse resolution of the compass all the time. |
386 | // The correction should really only serve to compensate for gyro drift. |
392 | // The correction should really only serve to compensate for gyro drift. |
387 | if(abs(error) < GYRO_DEG_FACTOR_YAW) return; |
393 | if(abs(error) < GYRO_DEG_FACTOR_YAW) return; |
388 | 394 | ||
- | 395 | int32_t correction = (error * staticParams.compassYawCorrection) >> 8; |
|
389 | int32_t correction = (error * (int32_t)dynamicParams.compassYawEffect) >> 8; |
396 | debugOut.analog[30] = correction; |
390 | 397 | ||
391 | // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
398 | // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
392 | // and to the target heading (the direction to which it maneuvers to point). That means, this correction has |
399 | // and to the target heading (the direction to which it maneuvers to point). That means, this correction has |
393 | // no effect on control at all!!! It only has effect on the values of the two variables. However, these values |
400 | // no effect on control at all!!! It only has effect on the values of the two variables. However, these values |
394 | // could have effect on control elsewhere, like in compassControl.c . |
401 | // could have effect on control elsewhere, like in compassControl.c . |
395 | heading += correction; |
402 | heading += correction; |
396 | intervalWrap(&heading, YAWOVER360); |
403 | intervalWrap(&heading, YAWOVER360); |
- | 404 | ||
- | 405 | // If we want a transparent flight wrt. compass correction (meaning the copter does not change attitude all |
|
397 | 406 | // when the compass corrects the heading - it only corrects numbers!) we want to add: |
|
398 | targetHeading += correction; |
407 | // This will however cause drift to remain uncorrected! |
399 | intervalWrap(&targetHeading, YAWOVER360); |
- | |
400 | 408 | // headingError += correction; |
|
401 | debugOut.digital[1] ^= DEBUG_COMPASS; |
409 | debugOut.analog[29] = 0; |
402 | } |
410 | } |
403 | 411 | ||
404 | /************************************************************************ |
412 | /************************************************************************ |
405 | * Main procedure. |
413 | * Main procedure. |
406 | ************************************************************************/ |
414 | ************************************************************************/ |
407 | void calculateFlightAttitude(void) { |
415 | void calculateFlightAttitude(void) { |
408 | getAnalogData(); |
416 | getAnalogData(); |
409 | calculateAccVector(); |
417 | calculateAccVector(); |
410 | integrate(); |
418 | integrate(); |
411 | 419 | ||
412 | #ifdef ATTITUDE_USE_ACC_SENSORS |
420 | #ifdef ATTITUDE_USE_ACC_SENSORS |
413 | correctIntegralsByAcc0thOrder(); |
421 | correctIntegralsByAcc0thOrder(); |
414 | driftCorrection(); |
422 | driftCorrection(); |
415 | #endif |
423 | #endif |
416 | 424 | ||
417 | // We are done reading variables from the analog module. |
425 | // We are done reading variables from the analog module. |
418 | // Interrupt-driven sensor reading may restart. |
426 | // Interrupt-driven sensor reading may restart. |
419 | startAnalogConversionCycle(); |
427 | startAnalogConversionCycle(); |
420 | 428 | ||
421 | if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
429 | if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
422 | correctHeadingToMagnetic(); |
430 | correctHeadingToMagnetic(); |
423 | } |
431 | } |
424 | } |
432 | } |
425 | 433 | ||
426 | /* |
434 | /* |
427 | * This is part of an experiment to measure average sensor offsets caused by motor vibration, |
435 | * This is part of an experiment to measure average sensor offsets caused by motor vibration, |
428 | * and to compensate them away. It brings about some improvement, but no miracles. |
436 | * and to compensate them away. It brings about some improvement, but no miracles. |
429 | * As long as the left stick is kept in the start-motors position, the dynamic compensation |
437 | * As long as the left stick is kept in the start-motors position, the dynamic compensation |
430 | * will measure the effect of vibration, to use for later compensation. So, one should keep |
438 | * will measure the effect of vibration, to use for later compensation. So, one should keep |
431 | * the stick in the start-motors position for a few seconds, till all motors run (at the wrong |
439 | * the stick in the start-motors position for a few seconds, till all motors run (at the wrong |
432 | * speed unfortunately... must find a better way) |
440 | * speed unfortunately... must find a better way) |
433 | */ |
441 | */ |
434 | /* |
442 | /* |
435 | void attitude_startDynamicCalibration(void) { |
443 | void attitude_startDynamicCalibration(void) { |
436 | dynamicCalPitch = dynamicCalRoll = dynamicCalYaw = dynamicCalCount = 0; |
444 | dynamicCalPitch = dynamicCalRoll = dynamicCalYaw = dynamicCalCount = 0; |
437 | savedDynamicOffsetPitch = savedDynamicOffsetRoll = 1000; |
445 | savedDynamicOffsetPitch = savedDynamicOffsetRoll = 1000; |
438 | } |
446 | } |
439 | 447 | ||
440 | void attitude_continueDynamicCalibration(void) { |
448 | void attitude_continueDynamicCalibration(void) { |
441 | // measure dynamic offset now... |
449 | // measure dynamic offset now... |
442 | dynamicCalPitch += hiResPitchGyro; |
450 | dynamicCalPitch += hiResPitchGyro; |
443 | dynamicCalRoll += hiResRollGyro; |
451 | dynamicCalRoll += hiResRollGyro; |
444 | dynamicCalYaw += rawYawGyroSum; |
452 | dynamicCalYaw += rawYawGyroSum; |
445 | dynamicCalCount++; |
453 | dynamicCalCount++; |
446 | 454 | ||
447 | // Param6: Manual mode. The offsets are taken from Param7 and Param8. |
455 | // Param6: Manual mode. The offsets are taken from Param7 and Param8. |
448 | if (dynamicParams.UserParam6 || 1) { // currently always enabled. |
456 | if (dynamicParams.UserParam6 || 1) { // currently always enabled. |
449 | // manual mode |
457 | // manual mode |
450 | driftCompPitch = dynamicParams.UserParam7 - 128; |
458 | driftCompPitch = dynamicParams.UserParam7 - 128; |
451 | driftCompRoll = dynamicParams.UserParam8 - 128; |
459 | driftCompRoll = dynamicParams.UserParam8 - 128; |
452 | } else { |
460 | } else { |
453 | // use the sampled value (does not seem to work so well....) |
461 | // use the sampled value (does not seem to work so well....) |
454 | driftCompPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount; |
462 | driftCompPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount; |
455 | driftCompRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount; |
463 | driftCompRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount; |
456 | driftCompYaw = -dynamicCalYaw / dynamicCalCount; |
464 | driftCompYaw = -dynamicCalYaw / dynamicCalCount; |
457 | } |
465 | } |
458 | 466 | ||
459 | // keep resetting these meanwhile, to avoid accumulating errors. |
467 | // keep resetting these meanwhile, to avoid accumulating errors. |
460 | setStaticAttitudeIntegrals(); |
468 | setStaticAttitudeIntegrals(); |
461 | yawAngle = 0; |
469 | yawAngle = 0; |
462 | } |
470 | } |
463 | */ |
471 | */ |
464 | 472 |