Rev 387 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 387 | Rev 391 | ||
---|---|---|---|
Line 176... | Line 176... | ||
176 | MM3_calib.X_range = (x_max - x_min); |
176 | MM3_calib.X_range = (x_max - x_min); |
177 | MM3_calib.Y_range = (y_max - y_min); |
177 | MM3_calib.Y_range = (y_max - y_min); |
178 | MM3_calib.Z_range = (z_max - z_min); |
178 | MM3_calib.Z_range = (z_max - z_min); |
Line 179... | Line 179... | ||
179 | 179 | ||
180 | // Offset der Achsen |
180 | // Offset der Achsen |
181 | MM3_calib.X_off = (MM3_calib.X_range / 2) - x_max; |
181 | MM3_calib.X_off = (x_max + x_min) / 2; |
182 | MM3_calib.Y_off = (MM3_calib.Y_range / 2) - y_max; |
182 | MM3_calib.Y_off = (y_max + y_min) / 2; |
Line 183... | Line 183... | ||
183 | MM3_calib.Z_off = (MM3_calib.Z_range / 2) - z_max; |
183 | MM3_calib.Z_off = (z_max + z_min) / 2; |
184 | 184 | ||
185 | // und im EEProm abspeichern |
185 | // und im EEProm abspeichern |
Line 199... | Line 199... | ||
199 | 199 | ||
Line 200... | Line 200... | ||
200 | div_faktor = (uint16_t)EE_Parameter.UserParam1 * 8; |
200 | div_faktor = (uint16_t)EE_Parameter.UserParam1 * 8; |
201 | 201 | ||
- | 202 | // Berechung von sinus und cosinus |
|
202 | // Berechung von sinus und cosinus |
203 | MM3.NickGrad = (IntegralNick/div_faktor); |
203 | MM3.NickGrad = (IntegralNick/div_faktor); |
204 | MM3.NickGrad = asin_i(MM3.NickGrad); |
Line 204... | Line 205... | ||
204 | sin_nick = sin_f(MM3.NickGrad); |
205 | sin_nick = sin_f(MM3.NickGrad); |
- | 206 | cos_nick = cos_f(MM3.NickGrad); |
|
205 | cos_nick = cos_f(MM3.NickGrad); |
207 | |
206 | 208 | MM3.RollGrad = (IntegralRoll/div_faktor); |
|
Line 207... | Line 209... | ||
207 | MM3.RollGrad = (IntegralRoll/div_faktor); |
209 | MM3.RollGrad = asin_i(MM3.RollGrad); |
208 | sin_roll = sin_f(MM3.RollGrad); |
210 | sin_roll = sin_f(MM3.RollGrad); |
209 | cos_roll = cos_f(MM3.RollGrad); |
211 | cos_roll = cos_f(MM3.RollGrad); |
210 | 212 | ||
Line 211... | Line 213... | ||
211 | // Offset |
213 | // Offset |
212 | x_axis = (MM3.x_axis + MM3_calib.X_off); |
214 | x_axis = (MM3.x_axis - MM3_calib.X_off); |
213 | y_axis = (MM3.y_axis + MM3_calib.Y_off); |
215 | y_axis = (MM3.y_axis - MM3_calib.Y_off); |
214 | z_axis = (MM3.z_axis + MM3_calib.Z_off); |
216 | z_axis = (MM3.z_axis - MM3_calib.Z_off); |
Line 241... | Line 243... | ||
241 | 243 | ||
242 | y_corr = y_axis * cos_roll; |
244 | y_corr = y_axis * cos_roll; |
Line 243... | Line 245... | ||
243 | y_corr += z_axis * sin_roll; |
245 | y_corr += z_axis * sin_roll; |
- | 246 | ||
244 | 247 | // Winkelberechnung |
|
- | 248 | ||
- | 249 | heading = atan2_i(x_corr, y_corr); |
|
Line 245... | Line 250... | ||
245 | // Winkelberechnung |
250 | if (heading < 0) heading = -heading; |
246 | heading = atan2_i(x_corr, y_corr); |
251 | else heading = 360 - heading; |
247 | 252 | ||
Line 248... | Line 253... | ||
248 | /* |
253 | /* |
249 | if (!x_corr && y_corr <0) return (90); |
254 | if (!x_corr && y_corr <0) return (90); |
250 | if (!x_corr && y_corr >0) return (270); |
255 | if (!x_corr && y_corr >0) return (270); |
251 | |
256 | |
252 | heading = atan(y_corr/x_corr)*57.29578; |
257 | heading = atan(y_corr/x_corr)*57.29578; |
253 | if (x_corr < 0) heading = 180-heading; |
- | |
254 | if (x_corr > 0 && y_corr < 0) heading = -heading; |
258 | if (x_corr < 0) heading = 180-heading; |
255 | if (x_corr > 0 && y_corr > 0) heading = 360 - heading; |
259 | if (x_corr > 0 && y_corr < 0) heading = -heading; |