Rev 787 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 787 | Rev 851 | ||
|---|---|---|---|
| Line 192... | Line 192... | ||
| 192 | cli(); |
192 | cli(); |
| 193 | mm3_x_axis = MM3.x_axis; |
193 | mm3_x_axis = MM3.x_axis; |
| 194 | mm3_y_axis = MM3.y_axis; |
194 | mm3_y_axis = MM3.y_axis; |
| 195 | mm3_z_axis = MM3.z_axis; |
195 | mm3_z_axis = MM3.z_axis; |
| 196 | SREG = tmp_sreg; |
196 | SREG = tmp_sreg; |
| 197 | 197 | /* |
|
| - | 198 | int temp = Aktuell_az-550; |
|
| - | 199 | DebugOut.Analog[2] = temp; |
|
| 198 | // Lage-Berechnung mittels Acc-Messwerte |
200 | // Lage-Berechnung mittels Acc-Messwerte |
| 199 | tilt = atan2_i(Aktuell_az-acc_neutral.C,AdWertAccNick*64); |
201 | tilt = atan2_i(temp,AdWertAccNick*64); |
| - | 202 | DebugOut.Analog[0] = tilt; |
|
| 200 | sin_nick = sin_i(tilt); |
203 | sin_nick = sin_i(tilt); |
| 201 | cos_nick = cos_i(tilt); |
204 | cos_nick = cos_i(tilt); |
| Line 202... | Line 205... | ||
| 202 | 205 | |
|
| - | 206 | tilt = atan2_i(temp,AdWertAccRoll*64); |
|
| 203 | tilt = atan2_i(Aktuell_az-acc_neutral.C,AdWertAccRoll*64); |
207 | DebugOut.Analog[1] = tilt; |
| 204 | sin_roll = sin_i(tilt); |
208 | sin_roll = sin_i(tilt); |
| 205 | cos_roll = cos_i(tilt); |
209 | cos_roll = cos_i(tilt); |
| 206 | /* |
210 | */ |
| 207 | // Lage-Berechnung mittels Gyro-Integral |
211 | // Lage-Berechnung mittels Gyro-Integral |
| 208 | uint16_t div_faktor; |
212 | uint16_t div_faktor; |
| Line 209... | Line 213... | ||
| 209 | div_faktor = (uint16_t)EE_Parameter.UserParam3 *8; |
213 | div_faktor = (uint16_t)EE_Parameter.UserParam3 *8; |
| Line 213... | Line 217... | ||
| 213 | cos_nick = cos_i(tilt); |
217 | cos_nick = cos_i(tilt); |
| Line 214... | Line 218... | ||
| 214 | 218 | ||
| 215 | tilt = (IntegralRoll /div_faktor); |
219 | tilt = (IntegralRoll /div_faktor); |
| 216 | sin_roll = sin_i(tilt); |
220 | sin_roll = sin_i(tilt); |
| 217 | cos_roll = cos_i(tilt); |
221 | cos_roll = cos_i(tilt); |
| 218 | */ |
222 | |
| 219 | // Offset und Normalisierung |
223 | // Offset und Normalisierung |
| 220 | Hx = (((int32_t)(mm3_x_axis - MM3_calib.X_off)) *512) /MM3_calib.X_range; |
224 | Hx = (((int32_t)(mm3_x_axis - MM3_calib.X_off)) *512) /MM3_calib.X_range; |
| 221 | Hy = (((int32_t)(mm3_y_axis - MM3_calib.Y_off)) *512) /MM3_calib.Y_range; |
225 | Hy = (((int32_t)(mm3_y_axis - MM3_calib.Y_off)) *512) /MM3_calib.Y_range; |