Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 229 → Rev 232

/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);