Rev 2041 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2041 | Rev 2045 | ||
---|---|---|---|
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | // + Copyright (c) 04.2007 Holger Buss |
2 | // + Copyright (c) 04.2007 Holger Buss |
3 | // + Nur f�r den privaten Gebrauch |
3 | // + Nur f�r den privaten Gebrauch |
4 | // + www.MikroKopter.com |
4 | // + www.MikroKopter.com |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
6 | // + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation), |
6 | // + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation), |
7 | // + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zul�ssig ist. |
7 | // + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zul�ssig ist. |
8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen, |
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen, |
11 | // + Verkauf von Luftbildaufnahmen, usw. |
11 | // + Verkauf von Luftbildaufnahmen, usw. |
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
13 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht, |
13 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht, |
14 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen |
14 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen |
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
17 | // + auf anderen Webseiten oder Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
17 | // + auf anderen Webseiten oder Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
18 | // + eindeutig als Ursprung verlinkt und genannt werden |
18 | // + eindeutig als Ursprung verlinkt und genannt werden |
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
20 | // + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion |
20 | // + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion |
21 | // + Benutzung auf eigene Gefahr |
21 | // + Benutzung auf eigene Gefahr |
22 | // + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den |
22 | // + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den |
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
24 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
24 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
25 | // + mit unserer Zustimmung zul�ssig |
25 | // + mit unserer Zustimmung zul�ssig |
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
30 | // + this list of conditions and the following disclaimer. |
30 | // + this list of conditions and the following disclaimer. |
31 | // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
31 | // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
32 | // + from this software without specific prior written permission. |
32 | // + from this software without specific prior written permission. |
33 | // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
33 | // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
34 | // + for non-commercial use (directly or indirectly) |
34 | // + for non-commercial use (directly or indirectly) |
35 | // + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
35 | // + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
36 | // + with our written permission |
36 | // + with our written permission |
37 | // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
37 | // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
38 | // + clearly linked as origin |
38 | // + clearly linked as origin |
39 | // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
39 | // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
40 | // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
40 | // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
41 | // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
41 | // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
42 | // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
42 | // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
43 | // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
43 | // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
44 | // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
44 | // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
45 | // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
45 | // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
46 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
46 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
47 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
47 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
48 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
48 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
49 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
49 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
50 | // + POSSIBILITY OF SUCH DAMAGE. |
50 | // + POSSIBILITY OF SUCH DAMAGE. |
51 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
51 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
52 | /************************************************************************/ |
52 | /************************************************************************/ |
53 | /* Flight Attitude */ |
53 | /* Flight Attitude */ |
54 | /************************************************************************/ |
54 | /************************************************************************/ |
55 | 55 | ||
56 | #include <stdlib.h> |
56 | #include <stdlib.h> |
57 | #include <avr/io.h> |
57 | #include <avr/io.h> |
58 | 58 | ||
59 | #include "attitude.h" |
59 | #include "attitude.h" |
60 | #include "dongfangMath.h" |
60 | #include "dongfangMath.h" |
61 | 61 | ||
62 | // For scope debugging only! |
62 | // For scope debugging only! |
63 | #include "rc.h" |
63 | #include "rc.h" |
64 | 64 | ||
65 | // where our main data flow comes from. |
65 | // where our main data flow comes from. |
66 | #include "analog.h" |
66 | #include "analog.h" |
67 | 67 | ||
68 | #include "configuration.h" |
68 | #include "configuration.h" |
69 | #include "output.h" |
69 | #include "output.h" |
70 | 70 | ||
71 | // Some calculations are performed depending on some stick related things. |
71 | // Some calculations are performed depending on some stick related things. |
72 | #include "controlMixer.h" |
72 | #include "controlMixer.h" |
73 | 73 | ||
74 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
74 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
75 | 75 | ||
76 | /* |
76 | /* |
77 | * Gyro readings, as read from the analog module. It would have been nice to flow |
77 | * Gyro readings, as read from the analog module. It would have been nice to flow |
78 | * them around between the different calculations as a struct or array (doing |
78 | * them around between the different calculations as a struct or array (doing |
79 | * things functionally without side effects) but this is shorter and probably |
79 | * things functionally without side effects) but this is shorter and probably |
80 | * faster too. |
80 | * faster too. |
81 | * The variables are overwritten at each attitude calculation invocation - the values |
81 | * The variables are overwritten at each attitude calculation invocation - the values |
82 | * are not preserved or reused. |
82 | * are not preserved or reused. |
83 | */ |
83 | */ |
84 | int16_t rate_ATT[2], yawRate; |
84 | int16_t rate_ATT[2], yawRate; |
85 | 85 | ||
86 | // With different (less) filtering |
86 | // With different (less) filtering |
87 | int16_t rate_PID[2]; |
87 | int16_t rate_PID[2]; |
88 | int16_t differential[2]; |
88 | int16_t differential[2]; |
89 | 89 | ||
90 | /* |
90 | /* |
91 | * Gyro readings, after performing "axis coupling" - that is, the transfomation |
91 | * Gyro readings, after performing "axis coupling" - that is, the transfomation |
92 | * of rotation rates from the airframe-local coordinate system to a ground-fixed |
92 | * of rotation rates from the airframe-local coordinate system to a ground-fixed |
93 | * coordinate system. If axis copling is disabled, the gyro readings will be |
93 | * coordinate system. If axis copling is disabled, the gyro readings will be |
94 | * copied into these directly. |
94 | * copied into these directly. |
95 | * These are global for the same pragmatic reason as with the gyro readings. |
95 | * These are global for the same pragmatic reason as with the gyro readings. |
96 | * The variables are overwritten at each attitude calculation invocation - the values |
96 | * The variables are overwritten at each attitude calculation invocation - the values |
97 | * are not preserved or reused. |
97 | * are not preserved or reused. |
98 | */ |
98 | */ |
99 | int16_t ACRate[2], ACYawRate; |
99 | int16_t ACRate[2], ACYawRate; |
100 | 100 | ||
101 | /* |
101 | /* |
102 | * Gyro integrals. These are the rotation angles of the airframe compared to the |
102 | * Gyro integrals. These are the rotation angles of the airframe compared to the |
103 | * horizontal plane, yaw relative to yaw at start. |
103 | * horizontal plane, yaw relative to yaw at start. |
104 | */ |
104 | */ |
105 | int32_t angle[2], yawAngleDiff; |
105 | int32_t angle[2], yawAngleDiff; |
106 | 106 | ||
107 | int readingHeight = 0; |
107 | int readingHeight = 0; |
108 | 108 | ||
109 | // Yaw angle and compass stuff. |
109 | // Yaw angle and compass stuff. |
110 | 110 | ||
111 | // This is updated/written from MM3. Negative angle indicates invalid data. |
111 | // This is updated/written from MM3. Negative angle indicates invalid data. |
112 | int16_t magneticHeading = -1; |
112 | int16_t magneticHeading = -1; |
113 | 113 | ||
114 | // This is NOT updated from MM3. Negative angle indicates invalid data. |
114 | // This is NOT updated from MM3. Negative angle indicates invalid data. |
115 | int16_t compassCourse = -1; |
115 | int16_t compassCourse = -1; |
116 | 116 | ||
117 | // The difference between the above 2 (heading - course) on a -180..179 degree interval. |
117 | // The difference between the above 2 (heading - course) on a -180..179 degree interval. |
118 | // Not necessary. Never read anywhere. |
118 | // Not necessary. Never read anywhere. |
119 | // int16_t compassOffCourse = 0; |
119 | // int16_t compassOffCourse = 0; |
120 | 120 | ||
121 | uint8_t updateCompassCourse = 0; |
121 | uint8_t updateCompassCourse = 0; |
122 | uint8_t compassCalState = 0; |
122 | uint8_t compassCalState = 0; |
123 | uint16_t ignoreCompassTimer = 500; |
123 | uint16_t ignoreCompassTimer = 500; |
124 | 124 | ||
125 | int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
125 | int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
126 | int16_t yawGyroDrift; |
126 | int16_t yawGyroDrift; |
127 | 127 | ||
128 | #define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L) |
128 | #define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L) |
129 | #define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L) |
129 | #define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L) |
130 | #define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L) |
130 | #define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L) |
131 | 131 | ||
132 | int16_t correctionSum[2] = { 0, 0 }; |
132 | int16_t correctionSum[2] = { 0, 0 }; |
133 | 133 | ||
134 | // For NaviCTRL use. |
134 | // For NaviCTRL use. |
135 | int16_t averageAcc[2] = { 0, 0 }, averageAccCount = 0; |
135 | int16_t averageAcc[2] = { 0, 0 }, averageAccCount = 0; |
136 | 136 | ||
137 | /* |
137 | /* |
138 | * Experiment: Compensating for dynamic-induced gyro biasing. |
138 | * Experiment: Compensating for dynamic-induced gyro biasing. |
139 | */ |
139 | */ |
140 | int16_t driftComp[2] = { 0, 0 }, driftCompYaw = 0; |
140 | int16_t driftComp[2] = { 0, 0 }, driftCompYaw = 0; |
141 | // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
141 | // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
142 | // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
142 | // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
143 | // int16_t dynamicCalCount; |
143 | // int16_t dynamicCalCount; |
144 | 144 | ||
145 | uint16_t accVector; |
145 | uint16_t accVector; |
146 | 146 | ||
147 | /************************************************************************ |
147 | /************************************************************************ |
148 | * Set inclination angles from the acc. sensor data. |
148 | * Set inclination angles from the acc. sensor data. |
149 | * If acc. sensors are not used, set to zero. |
149 | * If acc. sensors are not used, set to zero. |
150 | * TODO: One could use inverse sine to calculate the angles more |
150 | * TODO: One could use inverse sine to calculate the angles more |
151 | * accurately, but since: 1) the angles are rather small at times when |
151 | * accurately, but since: 1) the angles are rather small at times when |
152 | * it makes sense to set the integrals (standing on ground, or flying at |
152 | * it makes sense to set the integrals (standing on ground, or flying at |
153 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
153 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
154 | * it is hardly worth the trouble. |
154 | * it is hardly worth the trouble. |
155 | ************************************************************************/ |
155 | ************************************************************************/ |
156 | 156 | ||
157 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
157 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
158 | //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
158 | //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
159 | return (int32_t)GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis];// + correctionTerm; |
159 | return (int32_t)GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis];// + correctionTerm; |
160 | // return 342L * filteredAcc[axis]; |
160 | // return 342L * filteredAcc[axis]; |
161 | } |
161 | } |
162 | 162 | ||
163 | void setStaticAttitudeAngles(void) { |
163 | void setStaticAttitudeAngles(void) { |
164 | #ifdef ATTITUDE_USE_ACC_SENSORS |
164 | #ifdef ATTITUDE_USE_ACC_SENSORS |
165 | angle[PITCH] = getAngleEstimateFromAcc(PITCH); |
165 | angle[PITCH] = getAngleEstimateFromAcc(PITCH); |
166 | angle[ROLL] = getAngleEstimateFromAcc(ROLL); |
166 | angle[ROLL] = getAngleEstimateFromAcc(ROLL); |
167 | #else |
167 | #else |
168 | angle[PITCH] = angle[ROLL] = 0; |
168 | angle[PITCH] = angle[ROLL] = 0; |
169 | #endif |
169 | #endif |
170 | } |
170 | } |
171 | 171 | ||
172 | /************************************************************************ |
172 | /************************************************************************ |
173 | * Neutral Readings |
173 | * Neutral Readings |
174 | ************************************************************************/ |
174 | ************************************************************************/ |
175 | void attitude_setNeutral(void) { |
175 | void attitude_setNeutral(void) { |
176 | // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
176 | // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
177 | // dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0; |
177 | // dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0; |
178 | 178 | ||
179 | driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
179 | driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
180 | correctionSum[PITCH] = correctionSum[ROLL] = 0; |
180 | correctionSum[PITCH] = correctionSum[ROLL] = 0; |
181 | 181 | ||
182 | // Calibrate hardware. |
182 | // Calibrate hardware. |
183 | analog_setNeutral(); |
183 | analog_setNeutral(); |
184 | 184 | ||
185 | // reset gyro integrals to acc guessing |
185 | // reset gyro integrals to acc guessing |
186 | setStaticAttitudeAngles(); |
186 | setStaticAttitudeAngles(); |
187 | yawAngleDiff = 0; |
187 | yawAngleDiff = 0; |
188 | 188 | ||
189 | // update compass course to current heading |
189 | // update compass course to current heading |
190 | compassCourse = magneticHeading; |
190 | compassCourse = magneticHeading; |
191 | 191 | ||
192 | // Inititialize YawGyroIntegral value with current compass heading |
192 | // Inititialize YawGyroIntegral value with current compass heading |
193 | yawGyroHeading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
193 | yawGyroHeading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
194 | 194 | ||
195 | // Servo_On(); //enable servo output |
195 | // Servo_On(); //enable servo output |
196 | } |
196 | } |
197 | 197 | ||
198 | /************************************************************************ |
198 | /************************************************************************ |
199 | * Get sensor data from the analog module, and release the ADC |
199 | * Get sensor data from the analog module, and release the ADC |
200 | * TODO: Ultimately, the analog module could do this (instead of dumping |
200 | * TODO: Ultimately, the analog module could do this (instead of dumping |
201 | * the values into variables). |
201 | * the values into variables). |
202 | * The rate variable end up in a range of about [-1024, 1023]. |
202 | * The rate variable end up in a range of about [-1024, 1023]. |
203 | *************************************************************************/ |
203 | *************************************************************************/ |
204 | void getAnalogData(void) { |
204 | void getAnalogData(void) { |
205 | uint8_t axis; |
205 | uint8_t axis; |
206 | 206 | ||
207 | analog_update(); |
207 | analog_update(); |
208 | 208 | ||
209 | for (axis = PITCH; axis <= ROLL; axis++) { |
209 | for (axis = PITCH; axis <= ROLL; axis++) { |
210 | rate_PID[axis] = gyro_PID[axis] + driftComp[axis]; |
210 | rate_PID[axis] = gyro_PID[axis] + driftComp[axis]; |
211 | rate_ATT[axis] = gyro_ATT[axis] + driftComp[axis]; |
211 | rate_ATT[axis] = gyro_ATT[axis] + driftComp[axis]; |
212 | differential[axis] = gyroD[axis]; |
212 | differential[axis] = gyroD[axis]; |
213 | averageAcc[axis] += acc[axis]; |
213 | averageAcc[axis] += acc[axis]; |
214 | } |
214 | } |
215 | 215 | ||
216 | averageAccCount++; |
216 | averageAccCount++; |
217 | yawRate = yawGyro + driftCompYaw; |
217 | yawRate = yawGyro + driftCompYaw; |
218 | } |
218 | } |
219 | 219 | ||
220 | /* |
220 | /* |
221 | * This is the standard flight-style coordinate system transformation |
221 | * This is the standard flight-style coordinate system transformation |
222 | * (from airframe-local axes to a ground-based system). For some reason |
222 | * (from airframe-local axes to a ground-based system). For some reason |
223 | * the MK uses a left-hand coordinate system. The tranformation has been |
223 | * the MK uses a left-hand coordinate system. The tranformation has been |
224 | * changed accordingly. |
224 | * changed accordingly. |
225 | */ |
225 | */ |
226 | void trigAxisCoupling(void) { |
226 | void trigAxisCoupling(void) { |
- | 227 | int16_t rollAngleInDegrees = angle[ROLL] / GYRO_DEG_FACTOR_PITCHROLL; |
|
- | 228 | int16_t pitchAngleInDegrees = angle[PITCH] / GYRO_DEG_FACTOR_PITCHROLL; |
|
- | 229 | ||
227 | int16_t cospitch = int_cos(angle[PITCH]); |
230 | int16_t cospitch = cos_360(pitchAngleInDegrees); |
228 | int16_t cosroll = int_cos(angle[ROLL]); |
231 | int16_t cosroll = cos_360(rollAngleInDegrees); |
229 | int16_t sinroll = int_sin(angle[ROLL]); |
232 | int16_t sinroll = sin_360(rollAngleInDegrees); |
230 | 233 | ||
231 | ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate |
234 | ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate |
232 | * sinroll) >> MATH_UNIT_FACTOR_LOG); |
235 | * sinroll) >> MATH_UNIT_FACTOR_LOG); |
233 | 236 | ||
234 | ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll |
237 | ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll |
235 | + (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * int_tan( |
238 | + (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * tan_360(pitchAngleInDegrees)) >> MATH_UNIT_FACTOR_LOG); |
236 | angle[PITCH])) >> MATH_UNIT_FACTOR_LOG); |
- | |
237 | 239 | ||
238 | ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
240 | ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
239 | 241 | ||
240 | ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
242 | ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
241 | } |
243 | } |
242 | 244 | ||
243 | // 480 usec with axis coupling - almost no time without. |
245 | // 480 usec with axis coupling - almost no time without. |
244 | void integrate(void) { |
246 | void integrate(void) { |
245 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
247 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
246 | uint8_t axis; |
248 | uint8_t axis; |
247 | 249 | ||
248 | if (staticParams.bitConfig & CFG_AXIS_COUPLING_ACTIVE) { |
250 | if (staticParams.bitConfig & CFG_AXIS_COUPLING_ACTIVE) { |
249 | trigAxisCoupling(); |
251 | trigAxisCoupling(); |
250 | } else { |
252 | } else { |
251 | ACRate[PITCH] = rate_ATT[PITCH]; |
253 | ACRate[PITCH] = rate_ATT[PITCH]; |
252 | ACRate[ROLL] = rate_ATT[ROLL]; |
254 | ACRate[ROLL] = rate_ATT[ROLL]; |
253 | ACYawRate = yawRate; |
255 | ACYawRate = yawRate; |
254 | } |
256 | } |
255 | 257 | ||
256 | /* |
258 | /* |
257 | * Yaw |
259 | * Yaw |
258 | * Calculate yaw gyro integral (~ to rotation angle) |
260 | * Calculate yaw gyro integral (~ to rotation angle) |
259 | * Limit yawGyroHeading proportional to 0 deg to 360 deg |
261 | * Limit yawGyroHeading proportional to 0 deg to 360 deg |
260 | */ |
262 | */ |
261 | yawGyroHeading += ACYawRate; |
263 | yawGyroHeading += ACYawRate; |
262 | yawAngleDiff += yawRate; |
264 | yawAngleDiff += yawRate; |
263 | 265 | ||
264 | if (yawGyroHeading >= YAWOVER360) { |
266 | if (yawGyroHeading >= YAWOVER360) { |
265 | yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
267 | yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
266 | } else if (yawGyroHeading < 0) { |
268 | } else if (yawGyroHeading < 0) { |
267 | yawGyroHeading += YAWOVER360; |
269 | yawGyroHeading += YAWOVER360; |
268 | } |
270 | } |
269 | 271 | ||
270 | /* |
272 | /* |
271 | * Pitch axis integration and range boundary wrap. |
273 | * Pitch axis integration and range boundary wrap. |
272 | */ |
274 | */ |
273 | for (axis = PITCH; axis <= ROLL; axis++) { |
275 | for (axis = PITCH; axis <= ROLL; axis++) { |
274 | angle[axis] += ACRate[axis]; |
276 | angle[axis] += ACRate[axis]; |
275 | if (angle[axis] > PITCHROLLOVER180) { |
277 | if (angle[axis] > PITCHROLLOVER180) { |
276 | angle[axis] -= PITCHROLLOVER360; |
278 | angle[axis] -= PITCHROLLOVER360; |
277 | } else if (angle[axis] <= -PITCHROLLOVER180) { |
279 | } else if (angle[axis] <= -PITCHROLLOVER180) { |
278 | angle[axis] += PITCHROLLOVER360; |
280 | angle[axis] += PITCHROLLOVER360; |
279 | } |
281 | } |
280 | } |
282 | } |
281 | } |
283 | } |
282 | 284 | ||
283 | /************************************************************************ |
285 | /************************************************************************ |
284 | * A kind of 0'th order integral correction, that corrects the integrals |
286 | * A kind of 0'th order integral correction, that corrects the integrals |
285 | * directly. This is the "gyroAccFactor" stuff in the original code. |
287 | * directly. This is the "gyroAccFactor" stuff in the original code. |
286 | * There is (there) also a drift compensation |
288 | * There is (there) also a drift compensation |
287 | * - it corrects the differential of the integral = the gyro offsets. |
289 | * - it corrects the differential of the integral = the gyro offsets. |
288 | * That should only be necessary with drifty gyros like ENC-03. |
290 | * That should only be necessary with drifty gyros like ENC-03. |
289 | ************************************************************************/ |
291 | ************************************************************************/ |
290 | void correctIntegralsByAcc0thOrder(void) { |
292 | void correctIntegralsByAcc0thOrder(void) { |
291 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
293 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
292 | // are less than ....., or reintroduce Kalman. |
294 | // are less than ....., or reintroduce Kalman. |
293 | // Well actually the Z axis acc. check is not so silly. |
295 | // Well actually the Z axis acc. check is not so silly. |
294 | uint8_t axis; |
296 | uint8_t axis; |
295 | int32_t temp; |
297 | int32_t temp; |
296 | 298 | ||
297 | uint8_t ca = controlActivity >> 8; |
299 | uint8_t ca = controlActivity >> 8; |
298 | uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
300 | uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
299 | 301 | ||
300 | if (highControlActivity) { |
302 | if (highControlActivity) { |
301 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
303 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
302 | } else { |
304 | } else { |
303 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
305 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
304 | } |
306 | } |
305 | 307 | ||
306 | if (accVector <= dynamicParams.maxAccVector) { |
308 | if (accVector <= dynamicParams.maxAccVector) { |
307 | debugOut.digital[0] &= ~ DEBUG_ACC0THORDER; |
309 | debugOut.digital[0] &= ~ DEBUG_ACC0THORDER; |
308 | 310 | ||
309 | uint8_t permilleAcc = staticParams.zerothOrderCorrection; |
311 | uint8_t permilleAcc = staticParams.zerothOrderCorrection; |
310 | int32_t accDerived; |
312 | int32_t accDerived; |
311 | 313 | ||
312 | /* |
314 | /* |
313 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
315 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
314 | permilleAcc /= 2; |
316 | permilleAcc /= 2; |
315 | debugFullWeight = 0; |
317 | debugFullWeight = 0; |
316 | } |
318 | } |
317 | 319 | ||
318 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
320 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
319 | permilleAcc /= 2; |
321 | permilleAcc /= 2; |
320 | debugFullWeight = 0; |
322 | debugFullWeight = 0; |
321 | */ |
323 | */ |
322 | 324 | ||
323 | if (highControlActivity) { // reduce effect during stick control activity |
325 | if (highControlActivity) { // reduce effect during stick control activity |
324 | permilleAcc /= 4; |
326 | permilleAcc /= 4; |
325 | if (controlActivity > staticParams.maxControlActivity*2) { // reduce effect during stick control activity |
327 | if (controlActivity > staticParams.maxControlActivity*2) { // reduce effect during stick control activity |
326 | permilleAcc /= 4; |
328 | permilleAcc /= 4; |
327 | } |
329 | } |
328 | } |
330 | } |
329 | 331 | ||
330 | /* |
332 | /* |
331 | * Add to each sum: The amount by which the angle is changed just below. |
333 | * Add to each sum: The amount by which the angle is changed just below. |
332 | */ |
334 | */ |
333 | for (axis = PITCH; axis <= ROLL; axis++) { |
335 | for (axis = PITCH; axis <= ROLL; axis++) { |
334 | accDerived = getAngleEstimateFromAcc(axis); |
336 | accDerived = getAngleEstimateFromAcc(axis); |
335 | debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
337 | debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
336 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
338 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
337 | temp = angle[axis]; |
339 | temp = angle[axis]; |
338 | angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
340 | angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
339 | + (int32_t) permilleAcc * accDerived) / 1000L; |
341 | + (int32_t) permilleAcc * accDerived) / 1000L; |
340 | correctionSum[axis] += angle[axis] - temp; |
342 | correctionSum[axis] += angle[axis] - temp; |
341 | } |
343 | } |
342 | } else { |
344 | } else { |
343 | debugOut.analog[9] = 0; |
345 | debugOut.analog[9] = 0; |
344 | debugOut.analog[10] = 0; |
346 | debugOut.analog[10] = 0; |
345 | // experiment: Kill drift compensation updates when not flying smooth. |
347 | // experiment: Kill drift compensation updates when not flying smooth. |
346 | // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
348 | // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
347 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
349 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
348 | } |
350 | } |
349 | } |
351 | } |
350 | 352 | ||
351 | /************************************************************************ |
353 | /************************************************************************ |
352 | * This is an attempt to correct not the error in the angle integrals |
354 | * This is an attempt to correct not the error in the angle integrals |
353 | * (that happens in correctIntegralsByAcc0thOrder above) but rather the |
355 | * (that happens in correctIntegralsByAcc0thOrder above) but rather the |
354 | * cause of it: Gyro drift, vibration and rounding errors. |
356 | * cause of it: Gyro drift, vibration and rounding errors. |
355 | * All the corrections made in correctIntegralsByAcc0thOrder over |
357 | * All the corrections made in correctIntegralsByAcc0thOrder over |
356 | * DRIFTCORRECTION_TIME cycles are summed up. This number is |
358 | * DRIFTCORRECTION_TIME cycles are summed up. This number is |
357 | * then divided by DRIFTCORRECTION_TIME to get the approx. |
359 | * then divided by DRIFTCORRECTION_TIME to get the approx. |
358 | * correction that should have been applied to each iteration to fix |
360 | * correction that should have been applied to each iteration to fix |
359 | * the error. This is then added to the dynamic offsets. |
361 | * the error. This is then added to the dynamic offsets. |
360 | ************************************************************************/ |
362 | ************************************************************************/ |
361 | // 2 times / sec. = 488/2 |
363 | // 2 times / sec. = 488/2 |
362 | #define DRIFTCORRECTION_TIME 256L |
364 | #define DRIFTCORRECTION_TIME 256L |
363 | void driftCorrection(void) { |
365 | void driftCorrection(void) { |
364 | static int16_t timer = DRIFTCORRECTION_TIME; |
366 | static int16_t timer = DRIFTCORRECTION_TIME; |
365 | int16_t deltaCorrection; |
367 | int16_t deltaCorrection; |
366 | int16_t round; |
368 | int16_t round; |
367 | uint8_t axis; |
369 | uint8_t axis; |
368 | 370 | ||
369 | if (!--timer) { |
371 | if (!--timer) { |
370 | timer = DRIFTCORRECTION_TIME; |
372 | timer = DRIFTCORRECTION_TIME; |
371 | for (axis = PITCH; axis <= ROLL; axis++) { |
373 | for (axis = PITCH; axis <= ROLL; axis++) { |
372 | // Take the sum of corrections applied, add it to delta |
374 | // Take the sum of corrections applied, add it to delta |
373 | if (correctionSum[axis] >=0) |
375 | if (correctionSum[axis] >=0) |
374 | round = DRIFTCORRECTION_TIME / 2; |
376 | round = DRIFTCORRECTION_TIME / 2; |
375 | else |
377 | else |
376 | round = -DRIFTCORRECTION_TIME / 2; |
378 | round = -DRIFTCORRECTION_TIME / 2; |
377 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
379 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
378 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
380 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
379 | driftComp[axis] += deltaCorrection / staticParams.driftCompDivider; |
381 | driftComp[axis] += deltaCorrection / staticParams.driftCompDivider; |
380 | CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit); |
382 | CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit); |
381 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
383 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
382 | // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
384 | // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
383 | // debugOut.analog[28 + axis] = driftComp[axis]; |
385 | // debugOut.analog[28 + axis] = driftComp[axis]; |
384 | 386 | ||
385 | correctionSum[axis] = 0; |
387 | correctionSum[axis] = 0; |
386 | } |
388 | } |
387 | } |
389 | } |
388 | } |
390 | } |
389 | 391 | ||
390 | void calculateAccVector(void) { |
392 | void calculateAccVector(void) { |
391 | int16_t temp; |
393 | int16_t temp; |
392 | temp = filteredAcc[0] >> 3; |
394 | temp = filteredAcc[0] >> 3; |
393 | accVector = temp * temp; |
395 | accVector = temp * temp; |
394 | temp = filteredAcc[1] >> 3; |
396 | temp = filteredAcc[1] >> 3; |
395 | accVector += temp * temp; |
397 | accVector += temp * temp; |
396 | temp = filteredAcc[2] >> 3; |
398 | temp = filteredAcc[2] >> 3; |
397 | accVector += temp * temp; |
399 | accVector += temp * temp; |
398 | debugOut.analog[18] = accVector; |
400 | //debugOut.analog[18] = accVector; |
399 | } |
401 | } |
400 | 402 | ||
401 | /************************************************************************ |
403 | /************************************************************************ |
402 | * Main procedure. |
404 | * Main procedure. |
403 | ************************************************************************/ |
405 | ************************************************************************/ |
404 | void calculateFlightAttitude(void) { |
406 | void calculateFlightAttitude(void) { |
405 | getAnalogData(); |
407 | getAnalogData(); |
406 | calculateAccVector(); |
408 | calculateAccVector(); |
407 | integrate(); |
409 | integrate(); |
408 | 410 | ||
409 | #ifdef ATTITUDE_USE_ACC_SENSORS |
411 | #ifdef ATTITUDE_USE_ACC_SENSORS |
410 | correctIntegralsByAcc0thOrder(); |
412 | correctIntegralsByAcc0thOrder(); |
411 | driftCorrection(); |
413 | driftCorrection(); |
412 | #endif |
414 | #endif |
413 | 415 | ||
414 | // We are done reading variables from the analog module. |
416 | // We are done reading variables from the analog module. |
415 | // Interrupt-driven sensor reading may restart. |
417 | // Interrupt-driven sensor reading may restart. |
416 | startAnalogConversionCycle(); |
418 | startAnalogConversionCycle(); |
417 | } |
419 | } |
418 | 420 | ||
419 | void updateCompass(void) { |
421 | void updateCompass(void) { |
420 | int16_t w, v, r, correction, error; |
422 | int16_t w, v, r, correction, error; |
421 | 423 | ||
422 | if (compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) { |
424 | if (compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) { |
423 | if (controlMixer_testCompassCalState()) { |
425 | if (controlMixer_testCompassCalState()) { |
424 | compassCalState++; |
426 | compassCalState++; |
425 | if (compassCalState < 5) |
427 | if (compassCalState < 5) |
426 | beepNumber(compassCalState); |
428 | beepNumber(compassCalState); |
427 | else |
429 | else |
428 | beep(1000); |
430 | beep(1000); |
429 | } |
431 | } |
430 | } else { |
432 | } else { |
431 | // get maximum attitude angle |
433 | // get maximum attitude angle |
432 | w = abs(angle[PITCH] / 512); |
434 | w = abs(angle[PITCH] / 512); |
433 | v = abs(angle[ROLL] / 512); |
435 | v = abs(angle[ROLL] / 512); |
434 | if (v > w) |
436 | if (v > w) |
435 | w = v; |
437 | w = v; |
436 | correction = w / 8 + 1; |
438 | correction = w / 8 + 1; |
437 | // calculate the deviation of the yaw gyro heading and the compass heading |
439 | // calculate the deviation of the yaw gyro heading and the compass heading |
438 | if (magneticHeading < 0) |
440 | if (magneticHeading < 0) |
439 | error = 0; // disable yaw drift compensation if compass heading is undefined |
441 | error = 0; // disable yaw drift compensation if compass heading is undefined |
440 | else if (abs(yawRate) > 128) { // spinning fast |
442 | else if (abs(yawRate) > 128) { // spinning fast |
441 | error = 0; |
443 | error = 0; |
442 | } else { |
444 | } else { |
443 | // compassHeading - yawGyroHeading, on a -180..179 deg interval. |
445 | // compassHeading - yawGyroHeading, on a -180..179 deg interval. |
444 | error = ((540 + magneticHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) |
446 | error = ((540 + magneticHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) |
445 | % 360) - 180; |
447 | % 360) - 180; |
446 | } |
448 | } |
447 | if (!ignoreCompassTimer && w < 25) { |
449 | if (!ignoreCompassTimer && w < 25) { |
448 | yawGyroDrift += error; |
450 | yawGyroDrift += error; |
449 | // Basically this gets set if we are in "fix" mode, and when starting. |
451 | // Basically this gets set if we are in "fix" mode, and when starting. |
450 | if (updateCompassCourse) { |
452 | if (updateCompassCourse) { |
451 | beep(200); |
453 | beep(200); |
452 | yawGyroHeading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
454 | yawGyroHeading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
453 | compassCourse = magneticHeading; //(int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
455 | compassCourse = magneticHeading; //(int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
454 | updateCompassCourse = 0; |
456 | updateCompassCourse = 0; |
455 | } |
457 | } |
456 | } |
458 | } |
457 | yawGyroHeading += (error * 8) / correction; |
459 | yawGyroHeading += (error * 8) / correction; |
458 | 460 | ||
459 | /* |
461 | /* |
460 | w = (w * dynamicParams.CompassYawEffect) / 32; |
462 | w = (w * dynamicParams.CompassYawEffect) / 32; |
461 | w = dynamicParams.CompassYawEffect - w; |
463 | w = dynamicParams.CompassYawEffect - w; |
462 | */ |
464 | */ |
463 | w = dynamicParams.compassYawEffect - (w * dynamicParams.compassYawEffect) |
465 | w = dynamicParams.compassYawEffect - (w * dynamicParams.compassYawEffect) |
464 | / 32; |
466 | / 32; |
465 | 467 | ||
466 | // As readable formula: |
468 | // As readable formula: |
467 | // w = dynamicParams.CompassYawEffect * (1-w/32); |
469 | // w = dynamicParams.CompassYawEffect * (1-w/32); |
468 | 470 | ||
469 | if (w >= 0) { // maxAttitudeAngle < 32 |
471 | if (w >= 0) { // maxAttitudeAngle < 32 |
470 | if (!ignoreCompassTimer) { |
472 | if (!ignoreCompassTimer) { |
471 | /*v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;*/ |
473 | /*v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;*/ |
472 | v = 64 + controlActivity / 100; |
474 | v = 64 + controlActivity / 100; |
473 | // yawGyroHeading - compassCourse on a -180..179 degree interval. |
475 | // yawGyroHeading - compassCourse on a -180..179 degree interval. |
474 | r |
476 | r |
475 | = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) |
477 | = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) |
476 | % 360) - 180; |
478 | % 360) - 180; |
477 | v = (r * w) / v; // align to compass course |
479 | v = (r * w) / v; // align to compass course |
478 | // limit yaw rate |
480 | // limit yaw rate |
479 | w = 3 * dynamicParams.compassYawEffect; |
481 | w = 3 * dynamicParams.compassYawEffect; |
480 | if (v > w) |
482 | if (v > w) |
481 | v = w; |
483 | v = w; |
482 | else if (v < -w) |
484 | else if (v < -w) |
483 | v = -w; |
485 | v = -w; |
484 | yawAngleDiff += v; |
486 | yawAngleDiff += v; |
485 | } else { // wait a while |
487 | } else { // wait a while |
486 | ignoreCompassTimer--; |
488 | ignoreCompassTimer--; |
487 | } |
489 | } |
488 | } else { // ignore compass at extreme attitudes for a while |
490 | } else { // ignore compass at extreme attitudes for a while |
489 | ignoreCompassTimer = 500; |
491 | ignoreCompassTimer = 500; |
490 | } |
492 | } |
491 | } |
493 | } |
492 | } |
494 | } |
493 | 495 | ||
494 | /* |
496 | /* |
495 | * This is part of an experiment to measure average sensor offsets caused by motor vibration, |
497 | * This is part of an experiment to measure average sensor offsets caused by motor vibration, |
496 | * and to compensate them away. It brings about some improvement, but no miracles. |
498 | * and to compensate them away. It brings about some improvement, but no miracles. |
497 | * As long as the left stick is kept in the start-motors position, the dynamic compensation |
499 | * As long as the left stick is kept in the start-motors position, the dynamic compensation |
498 | * will measure the effect of vibration, to use for later compensation. So, one should keep |
500 | * will measure the effect of vibration, to use for later compensation. So, one should keep |
499 | * the stick in the start-motors position for a few seconds, till all motors run (at the wrong |
501 | * the stick in the start-motors position for a few seconds, till all motors run (at the wrong |
500 | * speed unfortunately... must find a better way) |
502 | * speed unfortunately... must find a better way) |
501 | */ |
503 | */ |
502 | /* |
504 | /* |
503 | void attitude_startDynamicCalibration(void) { |
505 | void attitude_startDynamicCalibration(void) { |
504 | dynamicCalPitch = dynamicCalRoll = dynamicCalYaw = dynamicCalCount = 0; |
506 | dynamicCalPitch = dynamicCalRoll = dynamicCalYaw = dynamicCalCount = 0; |
505 | savedDynamicOffsetPitch = savedDynamicOffsetRoll = 1000; |
507 | savedDynamicOffsetPitch = savedDynamicOffsetRoll = 1000; |
506 | } |
508 | } |
507 | 509 | ||
508 | void attitude_continueDynamicCalibration(void) { |
510 | void attitude_continueDynamicCalibration(void) { |
509 | // measure dynamic offset now... |
511 | // measure dynamic offset now... |
510 | dynamicCalPitch += hiResPitchGyro; |
512 | dynamicCalPitch += hiResPitchGyro; |
511 | dynamicCalRoll += hiResRollGyro; |
513 | dynamicCalRoll += hiResRollGyro; |
512 | dynamicCalYaw += rawYawGyroSum; |
514 | dynamicCalYaw += rawYawGyroSum; |
513 | dynamicCalCount++; |
515 | dynamicCalCount++; |
514 | 516 | ||
515 | // Param6: Manual mode. The offsets are taken from Param7 and Param8. |
517 | // Param6: Manual mode. The offsets are taken from Param7 and Param8. |
516 | if (dynamicParams.UserParam6 || 1) { // currently always enabled. |
518 | if (dynamicParams.UserParam6 || 1) { // currently always enabled. |
517 | // manual mode |
519 | // manual mode |
518 | driftCompPitch = dynamicParams.UserParam7 - 128; |
520 | driftCompPitch = dynamicParams.UserParam7 - 128; |
519 | driftCompRoll = dynamicParams.UserParam8 - 128; |
521 | driftCompRoll = dynamicParams.UserParam8 - 128; |
520 | } else { |
522 | } else { |
521 | // use the sampled value (does not seem to work so well....) |
523 | // use the sampled value (does not seem to work so well....) |
522 | driftCompPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount; |
524 | driftCompPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount; |
523 | driftCompRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount; |
525 | driftCompRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount; |
524 | driftCompYaw = -dynamicCalYaw / dynamicCalCount; |
526 | driftCompYaw = -dynamicCalYaw / dynamicCalCount; |
525 | } |
527 | } |
526 | 528 | ||
527 | // keep resetting these meanwhile, to avoid accumulating errors. |
529 | // keep resetting these meanwhile, to avoid accumulating errors. |
528 | setStaticAttitudeIntegrals(); |
530 | setStaticAttitudeIntegrals(); |
529 | yawAngle = 0; |
531 | yawAngle = 0; |
530 | } |
532 | } |
531 | */ |
533 | */ |
532 | 534 |