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; |