/branches/v0.60_MicroMag3_Nick666/compass.c |
---|
20,14 → 20,13 |
void init_spi(void) |
//############################################################################ |
{ |
SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0); |
SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0); //Interrupt an, Master, 156 kHz Oszillator |
//SPSR = (1<<SPI2X); |
DDRB |= (1<<PB2)|(1<<PB7)|(1<<PB5); // J8, SCK, MOSI Ausgang (MicroMag3) |
DDRB |= (1<<PB7)|(1<<PB5)|(1<<PB2); // J8, MOSI, SCK Ausgang |
MM3.AXIS = MM3_X; |
MM3.STATE = MM3_RESET; |
PORTD &= ~(1<<PD3); // J5 einmalig auf Low -> SSNOT |
} |
44,26 → 43,26 |
break; |
case MM3_START_TRANSFER: |
PORTB &= ~(1<<PB2); // J8 auf Low |
PORTB &= ~(1<<PB2); // J8 auf Low (war ~125 µs auf High) |
if (MM3.AXIS == MM3_X) SPDR = 0x41; // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus |
else if (MM3.AXIS == MM3_Y) SPDR = 0x42; |
else if (MM3.AXIS == MM3_Z) SPDR = 0x43; |
if (MM3.AXIS == MM3_X) SPDR = 0x51; // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus |
else if (MM3.AXIS == MM3_Y) SPDR = 0x52; // Micromag Period Select ist auf 1024 (0x50) |
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(8); |
MM3.DRDY = SetDelay(15); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 1024) |
MM3.STATE = MM3_WAIT_DRDY; |
break; |
case MM3_WAIT_DRDY: |
if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird |
if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei |
break; |
case MM3_IDLE: |
break; |
} |
} |
//############################################################################ |
//SPI byte ready |
72,28 → 71,59 |
{ |
switch (MM3.STATE) |
{ |
case MM3_DRDY: |
case MM3_DRDY: |
// 1. Byte ist da, abspeichern, an die MSB-Stelle rücken und Übertragung von 2. Byte auslösen |
if (MM3.AXIS == MM3_X) {MM3.x_axis=SPDR; MM3.x_axis<<=8; SPDR=0x00; MM3.STATE=MM3_X_BYTE2; break;} |
if (MM3.AXIS == MM3_Y) {MM3.y_axis=SPDR; MM3.y_axis<<=8; SPDR=0x00; MM3.STATE=MM3_Y_BYTE2; break;} |
if (MM3.AXIS == MM3_Z) {MM3.z_axis=SPDR; MM3.z_axis<<=8; SPDR=0x00; MM3.STATE=MM3_Z_BYTE2; break;} |
case MM3_X_BYTE2: |
MM3.x_axis |= SPDR; |
case MM3_X_BYTE2: // 2. Byte der entsprechenden Achse ist da. |
MM3.x_axis |= SPDR; |
MM3.x_axis -= OFF_X; // Sofort Offset aus der Kalibrierung berücksichtigen |
//MM3.x_axis /= GAIN_X; |
MM3.AXIS = MM3_Y; |
MM3.STATE = MM3_RESET; |
break; |
case MM3_Y_BYTE2: |
MM3.y_axis |= SPDR; |
MM3.y_axis |= SPDR; |
MM3.y_axis -= OFF_Y; |
//MM3.y_axis /= GAIN_Y; |
MM3.AXIS = MM3_Z; |
MM3.STATE = MM3_RESET; |
break; |
case MM3_Z_BYTE2: |
MM3.z_axis |= SPDR; |
MM3.z_axis |= SPDR; |
MM3.z_axis -= OFF_Z; |
//MM3.z_axis /= GAIN_Z; |
MM3.AXIS = MM3_X; |
MM3.STATE = MM3_RESET; |
MM3.STATE = MM3_RESET; |
// Zeitnahe Berechnung der Nick-/Rollneigung in ° |
MM3.NickGrad = IntegralNick/Int2Grad_Faktor; |
MM3.RollGrad = IntegralRoll/Int2Grad_Faktor; |
break; |
} |
} |
signed int MM3_heading(void) |
{ |
float sin_nick, cos_nick, sin_roll, cos_roll; |
signed int x_corr, y_corr; |
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; |
//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); |
return (heading); |
} |
/branches/v0.60_MicroMag3_Nick666/compass.h |
---|
2,12 → 2,14 |
typedef struct |
{ |
unsigned char STATE; |
uint8_t STATE; |
unsigned int DRDY; |
unsigned char AXIS; |
uint8_t AXIS; |
signed int x_axis; |
signed int y_axis; |
signed int z_axis; |
signed int NickGrad; |
signed int RollGrad; |
}MM3_struct; |
extern MM3_struct MM3; |
14,7 → 16,11 |
void init_spi(void); |
void MM3_timer0(void); |
signed int MM3_heading(void); |
#define Int2Grad_Faktor 1024 |
// Die Werte der Statemachine |
#define MM3_RESET 0 |
#define MM3_START_TRANSFER 1 |
#define MM3_WAIT_DRDY 2 |
27,8 → 33,16 |
#define MM3_Z 9 |
#define MM3_IDLE 255 |
#define PS2 6 |
#define PS1 5 |
#define PS0 5 |
#define AS1 1 |
#define AS0 0 |
// Die Werte der Kompasskalibrierung |
#define RANGE_X 1161 |
#define RANGE_Y 1209 |
#define RANGE_Z 1276 |
#define OFF_X -46 |
#define OFF_Y 52 |
#define OFF_Z 25 |
#define GAIN_X 14 |
#define GAIN_Y 15 |
#define GAIN_Z 16 |
/branches/v0.60_MicroMag3_Nick666/fc.c |
---|
679,7 → 679,13 |
DebugOut.Analog[5] = HoehenWert; |
DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
DebugOut.Analog[7] = GasMischanteil; |
DebugOut.Analog[8] = KompassValue; |
// DebugOut.Analog[8] = KompassValue; |
DebugOut.Analog[8] = MM3_heading(); |
DebugOut.Analog[9] = MM3.x_axis; |
DebugOut.Analog[10] = MM3.y_axis; |
DebugOut.Analog[11] = MM3.z_axis; |
// DebugOut.Analog[9] = SollHoehe; |
// DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
// DebugOut.Analog[11] = KompassStartwert; |
/branches/v0.60_MicroMag3_Nick666/math.c |
---|
13,14 → 13,16 |
#include "main.h" |
//----------------------------------------------- |
// Fast arctan2 with max error of .01 rads |
// Fast arctan2 with max error of .07 rads |
// http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm |
//----------------------------------------------- |
float arctan_f(signed int x, signed int y) |
signed char arctan_f(signed int x, signed int y) |
{ |
float rad,r; |
signed int angle; |
short int m; |
float rad,r; |
short int m; |
#define coeff_1 0.7854 |
#define coeff_2 2.3562 |
#define rad2grad 57.2958 |
if (!x && !y) return 0; |
29,15 → 31,17 |
if (x>=0) |
{ |
r = (x - y) / (x + y); |
rad = 0.1963*r*r*r - 0.9817*r + 0.7854; |
r = (float)(x - y) / (float)(x + y); |
rad = coeff_1 - coeff_1*r; |
} |
else |
{ |
r = (x + y) / (y - x); |
rad = 0.1963*r*r*r - 0.9817*r + 2.3562; |
r = (float)(x + y) / (float)(y - x); |
rad = coeff_2 - coeff_1*r; |
} |
rad *= rad2grad; |
return(rad*m); |
} |
47,9 → 51,8 |
Stand 1.10.2007 |
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
*/ |
// arctan Funktion: Eingabewert x,y Rueckgabe =arctan(x,y) in grad |
int8_t arctan_i( signed int x, signed int y) |
signed int arctan_i(signed int x, signed int y) |
{ |
short int change_xy = 0; |
signed int i; |
67,7 → 70,8 |
// Quadranten ermitteln |
// Wert durch lineare Interpolation ermitteln |
if ((y == 0) && (x == 0)) wert =1; // Division durch 0 nicht erlaubt |
if (!y && !x) return 0; // Division durch 0 nicht erlaubt |
else wert= abs((x*1000)/y); |
if (wert <=268) //0...0.0,268 entsprechend 0..15 Grad |
90,16 → 94,16 |
return return_value; |
} |
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) |
{ |
winkel = sin_i(90-winkel); |
return winkel; |
{ |
return (sin_i(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}; |
// sinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =sin(winkel)*1000 |
signed int sin_i(signed int winkel) |
{ |
126,3 → 130,51 |
} |
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)); |
} |
// sinus Funktion: Eingabewert Winkel in Grad |
float sin_f(signed int winkel) |
{ |
short int m,n; |
float sinus; |
//winkel = winkel % 360; |
if (winkel < 0) |
{ |
m = -1; |
winkel = abs(winkel); |
} |
else m = +1; |
// Quadranten auswerten |
if ((winkel > 90 ) && (winkel <= 180)) {winkel = 180 - winkel; n = 1;} |
else if ((winkel > 180 ) && (winkel <= 270)) {winkel = winkel - 180; n = -1;} |
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]); |
return (sinus*m*n); |
} |
/branches/v0.60_MicroMag3_Nick666/math.h |
---|
5,12 → 5,12 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#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 sin_i(signed int winkel); |
extern signed int cos_i(signed int winkel); |
extern int8_t arctan_i(signed int x, signed int y); |
extern float arctan_f(signed int x, signed int y); |
extern float cos_f(signed int winkel); |
extern float sin_f(signed int winkel); |
#define Pi 3.1416 |