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