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