Rev 716 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 716 | Rev 718 | ||
---|---|---|---|
Line 173... | Line 173... | ||
173 | //############################################################################ |
173 | //############################################################################ |
174 | // Neigungskompensierung und Berechnung der Ausrichtung |
174 | // Neigungskompensierung und Berechnung der Ausrichtung |
175 | int heading_MM3(void) |
175 | int heading_MM3(void) |
176 | //############################################################################ |
176 | //############################################################################ |
177 | { |
177 | { |
- | 178 | uint16_t div_faktor; |
|
178 | int16_t sin_nick, cos_nick, sin_roll, cos_roll; |
179 | int16_t sin_nick, cos_nick, sin_roll, cos_roll; |
179 | int32_t Hx, Hy, Hz; |
- | |
180 | int32_t x_corr, y_corr; |
180 | int32_t Hx, Hy, Hz, x_corr, y_corr; |
181 | int16_t heading; |
181 | int16_t heading; |
182 | int8_t nicktilt,rolltilt; |
182 | int8_t tilt; |
183 | 183 | /* |
|
184 | // Berechnung von sinus und cosinus |
184 | // Berechnung von sinus und cosinus |
185 | nicktilt = atan2_i(Aktuell_az-556,AdWertAccNick*64); |
185 | tilt = atan2_i(Aktuell_az-556,AdWertAccNick*64); |
186 | sin_nick = sin_i(nicktilt); |
186 | sin_nick = sin_i(tilt); |
187 | cos_nick = cos_i(nicktilt); |
187 | cos_nick = cos_i(tilt); |
188 | DebugOut.Analog[0] = nicktilt; |
- | |
Line 189... | Line 188... | ||
189 | 188 | |
|
190 | rolltilt = atan2_i(Aktuell_az-556,AdWertAccRoll*64); |
189 | tilt = atan2_i(Aktuell_az-556,AdWertAccRoll*64); |
191 | sin_roll = sin_i(rolltilt); |
190 | sin_roll = sin_i(tilt); |
- | 191 | cos_roll = cos_i(tilt); |
|
- | 192 | */ |
|
- | 193 | // calibration factor for transforming Gyro Integrals to angular degrees |
|
- | 194 | div_faktor = (uint16_t)EE_Parameter.UserParam3 *8; |
|
- | 195 | ||
- | 196 | // calculate sinus cosinus of pitch and tilt angle |
|
- | 197 | tilt = (IntegralNick/div_faktor); |
|
- | 198 | sin_nick = sin_i(tilt); |
|
- | 199 | cos_nick = cos_i(tilt); |
|
- | 200 | ||
- | 201 | tilt = (IntegralRoll/div_faktor); |
|
192 | cos_roll = cos_i(rolltilt); |
202 | sin_roll = sin_i(tilt); |
Line 193... | Line 203... | ||
193 | DebugOut.Analog[1] = rolltilt; |
203 | cos_roll = cos_i(tilt); |
194 | 204 | ||
195 | // Offset |
205 | // Offset |
196 | Hx = (MM3.x_axis - MM3_calib.X_off); |
206 | Hx = (MM3.x_axis - MM3_calib.X_off); |
197 | Hy = (MM3.y_axis - MM3_calib.Y_off); |
207 | Hy = (MM3.y_axis - MM3_calib.Y_off); |
198 | Hz = (MM3.z_axis - MM3_calib.Z_off); |
- | |
199 | 208 | Hz = (MM3.z_axis - MM3_calib.Z_off); |
|
200 | /* |
209 | |
201 | // Normierung Wertebereich |
210 | // Normierung Wertebereich |
202 | if ((MM3_calib.X_range > MM3_calib.Y_range) && (MM3_calib.X_range > MM3_calib.Z_range)) |
211 | if ((MM3_calib.X_range > MM3_calib.Y_range) && (MM3_calib.X_range > MM3_calib.Z_range)) |
203 | { |
212 | { |
Line 212... | Line 221... | ||
212 | else //if ((MM3_calib.Z_range > MM3_calib.X_range) && (MM3_calib.Z_range > MM3_calib.Y_range)) |
221 | else //if ((MM3_calib.Z_range > MM3_calib.X_range) && (MM3_calib.Z_range > MM3_calib.Y_range)) |
213 | { |
222 | { |
214 | Hx = (Hx * MM3_calib.Z_range) / MM3_calib.X_range; |
223 | Hx = (Hx * MM3_calib.Z_range) / MM3_calib.X_range; |
215 | Hy = (Hy * MM3_calib.Z_range) / MM3_calib.Y_range; |
224 | Hy = (Hy * MM3_calib.Z_range) / MM3_calib.Y_range; |
216 | } |
225 | } |
217 | */ |
- | |
Line 218... | Line 226... | ||
218 | 226 | ||
219 | // Neigungskompensierung |
227 | // Neigungskompensierung |
220 | x_corr = Hx * cos_nick; |
228 | x_corr = Hx * cos_nick; |
221 | x_corr -= Hz * sin_nick; |
229 | x_corr -= Hz * sin_nick; |