| 64,8 → 64,8 |
|---|
| return; // Jetzt gehts weiter in SIGNAL (SIG_SPI) |
| |
| case MM3_TILT: // Zeitnahe Speicherung der aktuellen Neigung in ° |
| MM3.NickGrad = asin_i((float)Aktuell_ax/215*200); |
| MM3.RollGrad = asin_i((float)Aktuell_ay/205*200); |
| MM3.NickGrad = asin_i((float)Aktuell_ax/208*200); |
| MM3.RollGrad = asin_i((float)Aktuell_ay/208*200); |
| |
| MM3.AXIS = MM3_X; |
| MM3.STATE = MM3_RESET; |
| 103,10 → 103,26 |
|---|
| return; |
| |
| case MM3_BYTE2: // 2. Byte der entsprechenden Achse ist da |
| |
| // versuch die werte nicht über min und max steigen zu lassen um so die spikes zu verhindern |
| // verhindert hats nicht aber deutlich minimiert. Evtl kommen die restlichen |
| |
| |
| if (MM3.AXIS == MM3_X) |
| { |
| MM3.x_axis |= SPDR; |
| MM3.x_axis -= OFF_X; // Sofort Offset aus der Kalibrierung berücksichtigen |
| if (MM3.x_axis > MM3_Xmax || MM3.x_axis < MM3_Xmin) |
| { |
| MM3.x_axis = MM3.x_axis_last_valid; |
| } |
| else |
| { |
| //m = (MM3.x_axis + MM3.x_axis_last_valid) /2; |
| MM3.x_axis_last_valid = MM3.x_axis; |
| //MM3.x_axis = m; |
| } |
| |
| MM3.AXIS = MM3_Y; |
| MM3.STATE = MM3_RESET; |
| } |
| 114,49 → 130,13 |
|---|
| { |
| MM3.y_axis |= SPDR; |
| MM3.y_axis -= OFF_Y; |
| MM3.AXIS = MM3_Z; |
| MM3.STATE = MM3_RESET; |
| } |
| else // if (MM3.AXIS == MM3_Z) |
| { |
| MM3.z_axis |= SPDR; |
| MM3.z_axis -= OFF_Z; |
| MM3.STATE = MM3_TILT; |
| } |
| |
| // versuch die werte nicht über min und max steigen zu lassen um so die spikes zu verhindern |
| // verhindert hats nicht aber deutlich minimiert. Evtl kommen die restlichen |
| |
| if (MM3.x_axis > MM3_Xmax || MM3.x_axis < MM3_Xmin) |
| { |
| MM3.x_axis = MM3.x_axis_last_valid; |
| } |
| else |
| if (MM3.x_axis_last_valid *1,10 < MM3.x_axis || MM3.x_axis_last_valid *0,90 > MM3.x_axis) |
| { |
| MM3.x_axis = MM3.x_axis_last_valid; |
| } |
| else |
| { |
| //m = (MM3.x_axis + MM3.x_axis_last_valid) /2; |
| MM3.x_axis_last_valid = MM3.x_axis; |
| //MM3.x_axis = m; |
| } |
| |
| |
| |
| |
| if (MM3.y_axis > MM3_Ymax || MM3.y_axis < MM3_Ymin) |
| { |
| MM3.y_axis = MM3.y_axis_last_valid; |
| } |
| else |
| if (MM3.y_axis_last_valid *1,10 < MM3.y_axis || MM3.y_axis_last_valid *0,90 > MM3.y_axis) |
| { |
| MM3.y_axis = MM3.y_axis_last_valid; |
| } |
| else |
| { |
| //m = (MM3.y_axis + MM3.y_axis_last_valid) /2; |
| MM3.y_axis_last_valid = MM3.y_axis; |
| //MM3.y_axis = m; |
| 163,29 → 143,27 |
|---|
| |
| } |
| |
| MM3.AXIS = MM3_Z; |
| MM3.STATE = MM3_RESET; |
| } |
| else // if (MM3.AXIS == MM3_Z) |
| { |
| MM3.z_axis |= SPDR; |
| MM3.z_axis -= OFF_Z; |
| |
| |
| if (MM3.z_axis > MM3_Zmax || MM3.z_axis < MM3_Zmin) |
| { |
| MM3.z_axis = MM3.z_axis_last_valid; |
| } |
| else |
| if (MM3.z_axis_last_valid *1,10 < MM3.z_axis || MM3.z_axis_last_valid *0,90 > MM3.z_axis) |
| { |
| MM3.z_axis = MM3.z_axis_last_valid; |
| } |
| else |
| { |
| //m = (MM3.z_axis + MM3.z_axis_last_valid) /2; |
| MM3.z_axis_last_valid = MM3.z_axis; |
| //MM3.z_axis = m; |
| } |
| |
| |
| |
| |
| |
| |
| MM3.STATE = MM3_TILT; |
| } |
| return; |
| } |
| } |