Rev 387 | Go to most recent revision | Show entire file | Ignore 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; |