/branches/v0.60_MicroMag3_Nick666/compass.c |
---|
50,7 → 50,7 |
else if (MM3.AXIS == MM3_Z) SPDR = 0x53; // 1: x-Achse, 2: Y-Achse, 3: Z-Achse |
else {MM3.STATE == MM3_IDLE;break;} |
MM3.DRDY = SetDelay(10); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 1024) |
MM3.DRDY = SetDelay(15); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 1024) |
MM3.STATE = MM3_WAIT_DRDY; |
break; |
100,8 → 100,8 |
MM3.AXIS = MM3_X; |
MM3.STATE = MM3_RESET; |
// Zeitnahe Speicherung der aktuellen Nick-/Rollneigung in ° |
MM3.NickGrad = IntegralNick/Int2Grad_Faktor; |
MM3.RollGrad = IntegralRoll/Int2Grad_Faktor; |
MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8); |
MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8); |
break; |
} |
} |
121,9 → 121,8 |
//Neigungskompensation |
y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis)); |
x_corr = (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick) + (cos_nick * MM3.x_axis); |
//Winkelberechnung |
heading = arctan_f(x_corr, y_corr); |
heading = arctan_i(x_corr, y_corr); |
return (heading); |
} |
/branches/v0.60_MicroMag3_Nick666/math.c |
---|
12,6 → 12,7 |
#include "main.h" |
/* |
//----------------------------------------------- |
// Fast arctan2 with max error of .07 rads |
// http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm |
44,54 → 45,40 |
return(rad*m); |
} |
*/ |
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Peter Muehlenbrock |
arctan in brute-force Art |
Stand 1.10.2007 |
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
*/ |
// arctan Funktion: Eingabewert x,y Rueckgabe =arctan(x,y) in grad |
const uint8_t pgm_atan[270] PROGMEM = {0,1,2,3,5,6,7,8,9,10,11,12,13,15,16,17,18,19,20,21,22,23,24,25,26,27,27,28,29,30,31,32,33,33,34,35,36,37,37,38,39,39,40,41,41,42,43,43,44,44,45,46,46,47,47,48,48,49,49,50,50,51,51,52,52,52,53,53,54,54,54,55,55,56,56,56,57,57,57,58,58,58,59,59,59,60,60,60,60,61,61,61,61,62,62,62,62,63,63,63,63,64,64,64,64,65,65,65,65,65,66,66,66,66,66,67,67,67,67,67,67,68,68,68,68,68,68,69,69,69,69,69,69,69,70,70,70,70,70,70,70,70,71,71,71,71,71,71,71,71,72,72,72,72,72,72,72,72,72,73,73,73,73,73,73,73,73,73,73,74,74,74,74,74,74,74,74,74,74,74,74,75,75,75,75,75,75,75,75,75,75,75,75,75,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79}; |
signed int arctan_i(signed int x, signed int y) |
{ |
short int change_xy = 0; |
signed int i; |
long signed int wert; |
int return_value; |
if ((abs(x)) > (abs(y))) // x,y Werte vertauschen damit arctan <45 grad bleibt |
{ |
change_xy = 1; |
i = x; |
x = y; |
y = i; |
} |
int i,angle; |
int8_t m; |
// Quadranten ermitteln |
if (!x && !y) return 0; //atan2 = 0 für x und y = 0 |
if (y < 0) m=-1; |
else m=1; |
if (x==0) return (90*m); // atan2 = 90° für x = 0 |
i = abs(((float)y / x) * 50.0); // Berechne i für die Lookup table (Schrittweite atan(x) ist 0.02 -> *50) |
// Wert durch lineare Interpolation ermitteln |
if (!y && !x) return 0; // Division durch 0 nicht erlaubt |
if (i<270) angle = pgm_read_byte(&pgm_atan[i]); // Lookup für 1° bis 79° |
else if (i>5750) angle = 90; // Grenzwert ist 90° |
else if (i>=1910) angle = 89; // 89° bis 80° über Wertebereiche |
else if (i>=1150) angle = 88; |
else if (i>=820) angle = 87; |
else if (i>=640) angle = 86; |
else if (i>=520) angle = 85; |
else if (i>=440) angle = 84; |
else if (i>=380) angle = 83; |
else if (i>=335) angle = 82; |
else if (i>=299) angle = 81; |
else angle = 80; // (i>=270) |
else wert= abs((x*1000)/y); |
if (wert <=268) //0...0.0,268 entsprechend 0..15 Grad |
{ |
return_value = (signed int)((wert*100)/(268-0)*(15-0)/100) +0; |
} |
else if (wert <=578) //0,268...0.0,568 entsprechend 15..30 Grad |
{ |
return_value = (signed int)((((wert-268)*100)/(578-268)*(30-15))/100) +15; |
} |
else //0,568...1 entsprechend 30..45 Grad |
{ |
return_value = (signed int)((((wert-578)*50)/(1000-578)*(45-30))/50) +30; |
} |
if (change_xy == 0) return_value = 90-return_value; //Quadrant 45..90 Grad |
if ((x >= 0) && (y <0)) return_value = - return_value; |
else if ((x < 0) && (y >= 0)) return_value = - return_value; |
return return_value; |
if (x > 0) return (angle*m); // Quadrant I und IV |
else if ((x < 0) && (y >= 0)) return ((angle*-1) + 180); // Quadrant II |
else return (angle - 180); // x < 0 && y < 0 Quadrant III |
} |
147,10 → 134,8 |
} |
/* |
const unsigned int pgm_sinus[91] PROGMEM = {0,17,35,52,70,87,105,122,139,156,174,191,208,225,242,259,276,292,309,326,342,358,375,391,407,423,438,454,469,485,500,515,530,545,559,574,588,602,616,629,643,656,669,682,695,707,719,731,743,755,766,777,788,799,809,819,829,839,848,857,866,875,883,891,899,906,914,921,927,934,940,946,951,956,961,966,970,974,978,982,985,988,990,993,995,996,998,999,999,1000,1000}; |
// cosinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =cos(winkel)*1000 |
183,3 → 168,4 |
return (winkel*m*n); |
} |
*/ |
/branches/v0.60_MicroMag3_Nick666/timer0.c |
---|
36,7 → 36,7 |
if(beeptime > 1) |
{ |
beeptime--; |
PORTD |= (1<<2); |
PORTD |= (1<<PD2); |
} |
else |
PORTD &= ~(1<<PD2); |