Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 228 → Rev 229

/branches/v0.60_MicroMag3_Nick666/compass.c
113,10 → 113,10
signed int heading;
//Berechung von sinus und cosinus
sin_nick = (float)sin_i(MM3.NickGrad)/1000;
cos_nick = (float)cos_i(MM3.NickGrad)/1000;
sin_roll = (float)sin_i(MM3.RollGrad)/1000;
cos_roll = (float)cos_i(MM3.RollGrad)/1000;
sin_nick = (float)sin_f(MM3.NickGrad);
cos_nick = (float)cos_f(MM3.NickGrad);
sin_roll = (float)sin_f(MM3.RollGrad);
cos_roll = (float)cos_f(MM3.RollGrad);
//Neigungskompensation
y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis));
/branches/v0.60_MicroMag3_Nick666/math.c
16,7 → 16,7
// Fast arctan2 with max error of .07 rads
// http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm
//-----------------------------------------------
signed char arctan_f(signed int x, signed int y)
signed int arctan_f(signed int x, signed int y)
{
float rad,r;
short int m;
96,18 → 96,35
 
 
 
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
signed int cos_i(signed int winkel)
{
return (sin_i(90-winkel));
 
const float pgm_sinus_f [91] PROGMEM = {0.000,0.017,0.035,0.052,0.070,0.087,0.105,0.122,0.139,0.156,0.174,0.191,0.208,0.225,0.242,0.259,0.276,0.292,0.309,0.326,0.342,0.358,0.375,0.391,0.407,0.423,0.438,0.454,0.469,0.485,0.500,0.515,0.530,0.545,0.559,0.574,0.588,0.602,0.616,0.629,0.643,0.656,0.669,0.682,0.695,0.707,0.719,0.731,0.743,0.755,0.766,0.777,0.788,0.799,0.809,0.819,0.829,0.839,0.848,0.857,0.866,0.875,0.883,0.891,0.899,0.906,0.914,0.921,0.927,0.934,0.940,0.946,0.951,0.956,0.961,0.966,0.970,0.974,0.978,0.982,0.985,0.988,0.990,0.993,0.995,0.996,0.998,0.999,0.999,1.000,1.000};
 
inline float pgm_read_float(const float *addr)
{
union
{
uint16_t i[2]; // 2 16-bit-Worte
float f;
} u;
u.i[0]=pgm_read_word((PGM_P)addr);
u.i[1]=pgm_read_word((PGM_P)addr+2);
return u.f;
}
 
// cosinus Funktion: Eingabewert Winkel in Grad
float cos_f(signed int winkel)
{
return (sin_f(90-winkel));
}
 
// sinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =sin(winkel)*1000
signed int sin_i(signed int winkel)
// sinus Funktion: Eingabewert Winkel in Grad
float sin_f(signed int winkel)
{
short int m,n;
float sinus;
//winkel = winkel % 360;
124,40 → 141,28
else if ((winkel > 270) && (winkel <= 360)) {winkel = 360 - winkel; n = -1;}
else n = 1; //0 - 90 Grad
 
winkel = pgm_read_word(&pgm_sinus[winkel]);
sinus = pgm_read_float(&pgm_sinus_f[winkel]);
 
return (winkel*m*n);
return (sinus*m*n);
}
 
 
 
const float pgm_sinus_f [91] PROGMEM = {0.00,0.02,0.03,0.05,0.07,0.09,0.10,0.12,0.14,0.16,0.17,0.19,0.21,0.23,0.24,0.26,0.28,0.29,0.31,0.33,0.34,0.36,0.38,0.39,0.41,0.42,0.44,0.45,0.47,0.49,0.50,0.52,0.53,0.55,0.56,0.57,0.59,0.60,0.62,0.63,0.64,0.66,0.67,0.68,0.70,0.71,0.72,0.73,0.74,0.76,0.77,0.78,0.79,0.80,0.81,0.82,0.83,0.84,0.85,0.86,0.87,0.88,0.88,0.89,0.90,0.91,0.91,0.92,0.93,0.93,0.94,0.95,0.95,0.96,0.96,0.97,0.97,0.97,0.98,0.98,0.98,0.99,0.99,0.99,0.99,1.00,1.00,1.00,1.00,1.00,1.00};
 
inline float pgm_read_float(const float *addr)
{
union
{
uint16_t i[2]; // 2 16-bit-Worte
float f;
} u;
u.i[0]=pgm_read_word((PGM_P)addr);
u.i[1]=pgm_read_word((PGM_P)addr+2);
return u.f;
}
 
// cosinus Funktion: Eingabewert Winkel in Grad
float cos_f(signed int winkel)
{
return (sin_f(90-winkel));
 
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
signed int cos_i(signed int winkel)
{
return (sin_i(90-winkel));
}
 
// sinus Funktion: Eingabewert Winkel in Grad
float sin_f(signed int winkel)
// sinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =sin(winkel)*1000
signed int sin_i(signed int winkel)
{
short int m,n;
float sinus;
//winkel = winkel % 360;
174,7 → 179,7
else if ((winkel > 270) && (winkel <= 360)) {winkel = 360 - winkel; n = -1;}
else n = 1; //0 - 90 Grad
 
sinus = pgm_read_float(&pgm_sinus_f[winkel]);
winkel = pgm_read_word(&pgm_sinus[winkel]);
 
return (sinus*m*n);
return (winkel*m*n);
}
/branches/v0.60_MicroMag3_Nick666/math.h
6,7 → 6,7
#include "main.h"
 
extern signed int arctan_i(signed int x, signed int y);
extern signed char arctan_f(signed int x, signed int y);
extern signed int arctan_f(signed int x, signed int y);
 
extern signed int sin_i(signed int winkel);
extern signed int cos_i(signed int winkel);