WebSVN - FlightCtrl - Diff - Rev 371 and 373 - /branches/v0.60_MicroMag3


/*
 
Copyright 2007, Niklas Nold
 
This program (files compass.c and compass.h) is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation;
either version 3 of the License, or (at your option) any later version.
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
 
Please note: All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de
*/
 
#include "main.h"
 
struct MM3_calib_struct ee_calib EEMEM;
 
struct MM3_working_struct MM3;
struct MM3_calib_struct MM3_calib;
 
 
//############################################################################
// Initialisierung
void init_MM3(void)
//############################################################################
{
        SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0);        //Interrupt an, Master, 156 kHz Oszillator
        //SPSR = (1<<SPI2X);
       
    DDRB |= (1<<PB7)|(1<<PB5)|(1<<PB2); // J8, MOSI, SCK Ausgang
       
        PORTD &= ~(1<<PD3);     // J5 auf Low
       
        MM3.AXIS = MM3_X;
        MM3.STATE = MM3_RESET;
       
        // Kalibrierung aus dem EEprom lesen
        eeprom_read_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct));
}
 
 
//############################################################################
// Wird in der SIGNAL (SIG_OVERFLOW0) aufgerufen
void timer0_MM3(void)
//############################################################################
{
        switch (MM3.STATE)
        {
        case MM3_RESET:                        
                PORTB |= (1<<PB2);      // J8 auf High, MM3 Reset
                MM3.STATE = MM3_START_TRANSFER;
                return;
               
        case MM3_START_TRANSFER:
                PORTB &= ~(1<<PB2);     // J8 auf Low (war ~125 µs auf High)            
               
                if (MM3.AXIS == MM3_X) SPDR = 0x31;                     // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus
                else if (MM3.AXIS == MM3_Y) SPDR = 0x32;                // Micromag Period Select ist auf 256 (0x30)
                else if (MM3.AXIS == MM3_Z) SPDR = 0x33;                // 1: x-Achse, 2: Y-Achse, 3: Z-Achse
               
                MM3.DRDY = SetDelay(8);         // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms)
                MM3.STATE = MM3_WAIT_DRDY;
                return;
       
        case MM3_WAIT_DRDY:
                if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei
                return;                                                 // Jetzt gehts weiter in SIGNAL (SIG_SPI)
        /*
        case MM3_TILT:                                          // Zeitnahe Speicherung der aktuellen Neigung in °
                MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8);
                MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8);
               
                MM3.AXIS = MM3_X;
                MM3.STATE = MM3_RESET;
                return;
        */
        }
}
 
 
//############################################################################
// SPI byte ready
SIGNAL (SIG_SPI)
//############################################################################
{      
        switch (MM3.STATE)
        {      
        case MM3_DRDY:          // 1. Byte ist da, abspeichern, an die MSB-Stelle rücken                
                if (MM3.AXIS == MM3_X)
                {
                        MM3.x_axis = SPDR;
                        MM3.x_axis <<= 8;
                }
                else if (MM3.AXIS == MM3_Y)
                {
                        MM3.y_axis = SPDR;
                        MM3.y_axis <<= 8;
                }
                else    // if (MM3.AXIS == MM3_Z)
                {
                        MM3.z_axis = SPDR;
                        MM3.z_axis <<= 8;
                }
               
                SPDR=0x00;              // Übertragung von 2. Byte auslösen
                MM3.STATE=MM3_BYTE2;
                return;
               
        case MM3_BYTE2:         // 2. Byte der entsprechenden Achse ist da              
                if (MM3.AXIS == MM3_X)
                {
                        MM3.x_axis |= SPDR;
                        // Spikes filtern
                        if (abs(MM3.x_axis) < Max_Axis_Value) MM3.x_axis_old = MM3.x_axis;
                        else MM3.x_axis = MM3.x_axis_old;
                        MM3.AXIS = MM3_Y;
                        MM3.STATE = MM3_RESET;
                }
                else if (MM3.AXIS == MM3_Y)
                {
                        MM3.y_axis |= SPDR;
                        if (abs(MM3.y_axis) < Max_Axis_Value) MM3.y_axis_old = MM3.y_axis;
                        else MM3.y_axis = MM3.y_axis_old;              
                        MM3.AXIS = MM3_Z;
                        MM3.STATE = MM3_RESET;
                }
                else    // if (MM3.AXIS == MM3_Z) 
                {
                        MM3.z_axis |= SPDR;
                        if (abs(MM3.z_axis) < Max_Axis_Value) MM3.z_axis_old = MM3.z_axis;
                        else MM3.z_axis = MM3.z_axis_old;
                        MM3.AXIS = MM3_X;
                        MM3.STATE = MM3_RESET;
                }
               
                return;
        }
}
 
//############################################################################
// Kompass kalibrieren
void calib_MM3(void)
//############################################################################
{
        signed int x_min=0,x_max=0,y_min=0,y_max=0,z_min=0,z_max=0;
        uint8_t measurement=50,beeper=0;
        unsigned int timer;
       
        GRN_ON;
        ROT_OFF;
       
        while (measurement)
        {
                //H_earth = MM3.x_axis*MM3.x_axis + MM3.y_axis*MM3.y_axis + MM3.z_axis*MM3.z_axis;
               
                if (MM3.x_axis > x_max) x_max = MM3.x_axis;
                else if (MM3.x_axis < x_min) x_min = MM3.x_axis;
               
                if (MM3.y_axis > y_max) y_max = MM3.y_axis;
                else if (MM3.y_axis < y_min) y_min = MM3.y_axis;
               
                if (MM3.z_axis > z_max) z_max = MM3.z_axis;
                else if (MM3.z_axis < z_min) z_min = MM3.z_axis;
               
                if (!beeper)
                {
                        ROT_FLASH;
                        GRN_FLASH;
                        //beeptime = 50;
                        beeper = 50;
                }
                beeper--;
               
                // Schleife mit 100 Hz
                timer = SetDelay(10);
                while(!CheckDelay(timer));
               
                // Wenn Gas zurück genommen wird, Kalibrierung mit 1/2 Sekunde Verzögerung beenden
                if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--;
        }
       
        // Offset der Achsen berechnen
        MM3_calib.X_off = (x_max + x_min) / 2;
        MM3_calib.Y_off = (y_max + y_min) / 2;
        MM3_calib.Z_off = (z_max + z_min) / 2;
       
        // und im EEProm abspeichern
        eeprom_write_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct));
       
}
 
 
//############################################################################
// Neigungskompensierung und Berechnung der Ausrichtung
signed int heading_MM3(void)
//############################################################################
{
        float sin_nick, cos_nick, sin_roll, cos_roll;
        signed int x_corr, y_corr, heading;
        signed int x_axis,y_axis,z_axis;
       
        MM3.NickGrad = -(IntegralNick/(EE_Parameter.UserParam1*8));
        MM3.RollGrad = -(IntegralRoll/(EE_Parameter.UserParam2*8));
       
        // Berechung von sinus und cosinus
        sin_nick = sin_f(MM3.NickGrad);
        cos_nick = cos_f(MM3.NickGrad);
        sin_roll = sin_f(MM3.RollGrad);
        cos_roll = cos_f(MM3.RollGrad);
 
        // Offset der Achsen nur bei Bedarf (also hier) berücksichtigen
        x_axis = -(MM3.x_axis - MM3_calib.X_off);
        y_axis = -(MM3.y_axis - MM3_calib.Y_off);
        z_axis = -(MM3.z_axis - MM3_calib.Z_off);      
       
    // Neigungskompensation
        x_corr = (cos_nick * x_axis) + (((sin_roll *  y_axis) - (cos_roll * z_axis)) * sin_nick);
        y_corr = ((cos_roll * y_axis) + (sin_roll * z_axis));
       
        // Winkelberechnung
        heading = atan2_i(x_corr, y_corr);
 
return (heading);
}
 

Rev 371	Rev 373
1	/*	1	/*
2		2
3	Copyright 2007, Niklas Nold	3	Copyright 2007, Niklas Nold
4		4
5	This program (files compass.c and compass.h) is free software; you can redistribute it and/or modify	5	This program (files compass.c and compass.h) is free software; you can redistribute it and/or modify
6	it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation;	6	it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation;
7	either version 3 of the License, or (at your option) any later version.	7	either version 3 of the License, or (at your option) any later version.
8	This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;	8	This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
9	without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the	9	without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10	GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License	10	GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License
11	along with this program. If not, see <http://www.gnu.org/licenses/>.	11	along with this program. If not, see <http://www.gnu.org/licenses/>.
12		12
13	Please note: All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de	13	Please note: All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de
14	*/	14	*/
15		15
16	#include "main.h"	16	#include "main.h"
17		17
18	MM3_working_struct MM3;	18	struct MM3_calib_struct ee_calib EEMEM;
19		19
20	MM3_calib_struct ee_calib EEMEM; // Reservierung im EEPROM	20	struct MM3_working_struct MM3;
21	MM3_calib_struct MM3_calib;	21	struct MM3_calib_struct MM3_calib;
22		22
23		23
24	//############################################################################	24	//############################################################################
25	// Initialisierung	25	// Initialisierung
26	void init_MM3(void)	26	void init_MM3(void)
27	//############################################################################	27	//############################################################################
28	{	28	{
29	SPCR = (1<<SPIE)\|(1<<SPE)\|(1<<MSTR)\|(1<<SPR1)\|(1<<SPR0); //Interrupt an, Master, 156 kHz Oszillator	29	SPCR = (1<<SPIE)\|(1<<SPE)\|(1<<MSTR)\|(1<<SPR1)\|(1<<SPR0); //Interrupt an, Master, 156 kHz Oszillator
30	//SPSR = (1<<SPI2X);	30	//SPSR = (1<<SPI2X);
31		31
32	DDRB \|= (1<<PB7)\|(1<<PB5)\|(1<<PB2); // J8, MOSI, SCK Ausgang	32	DDRB \|= (1<<PB7)\|(1<<PB5)\|(1<<PB2); // J8, MOSI, SCK Ausgang
33		33
34	PORTD &= ~(1<<PD3); // J5 auf Low	34	PORTD &= ~(1<<PD3); // J5 auf Low
35		35
36	MM3.AXIS = MM3_X;	36	MM3.AXIS = MM3_X;
37	MM3.STATE = MM3_RESET;	37	MM3.STATE = MM3_RESET;
38		38
39	// Kalibrierung aus dem EEprom lesen	39	// Kalibrierung aus dem EEprom lesen
40	eeprom_read_block(&MM3_calib,&ee_calib,sizeof(MM3_calib_struct));	40	eeprom_read_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct));
41	}	41	}
42		42
43		43
44	//############################################################################	44	//############################################################################
45	// Wird in der SIGNAL (SIG_OVERFLOW0) aufgerufen	45	// Wird in der SIGNAL (SIG_OVERFLOW0) aufgerufen
46	void timer0_MM3(void)	46	void timer0_MM3(void)
47	//############################################################################	47	//############################################################################
48	{	48	{
49	switch (MM3.STATE)	49	switch (MM3.STATE)
50	{	50	{
51	case MM3_RESET:	51	case MM3_RESET:
52	PORTB \|= (1<<PB2); // J8 auf High, MM3 Reset	52	PORTB \|= (1<<PB2); // J8 auf High, MM3 Reset
53	MM3.STATE = MM3_START_TRANSFER;	53	MM3.STATE = MM3_START_TRANSFER;
54	return;	54	return;
55		55
56	case MM3_START_TRANSFER:	56	case MM3_START_TRANSFER:
57	PORTB &= ~(1<<PB2); // J8 auf Low (war ~125 µs auf High)	57	PORTB &= ~(1<<PB2); // J8 auf Low (war ~125 µs auf High)
58		58
59	if (MM3.AXIS == MM3_X) SPDR = 0x31; // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus	59	if (MM3.AXIS == MM3_X) SPDR = 0x31; // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus
60	else if (MM3.AXIS == MM3_Y) SPDR = 0x32; // Micromag Period Select ist auf 256 (0x30)	60	else if (MM3.AXIS == MM3_Y) SPDR = 0x32; // Micromag Period Select ist auf 256 (0x30)
61	else if (MM3.AXIS == MM3_Z) SPDR = 0x33; // 1: x-Achse, 2: Y-Achse, 3: Z-Achse	61	else if (MM3.AXIS == MM3_Z) SPDR = 0x33; // 1: x-Achse, 2: Y-Achse, 3: Z-Achse
62		62
63	MM3.DRDY = SetDelay(8); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms)	63	MM3.DRDY = SetDelay(8); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms)
64	MM3.STATE = MM3_WAIT_DRDY;	64	MM3.STATE = MM3_WAIT_DRDY;
65	return;	65	return;
66		66
67	case MM3_WAIT_DRDY:	67	case MM3_WAIT_DRDY:
68	if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei	68	if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei
69	return; // Jetzt gehts weiter in SIGNAL (SIG_SPI)	69	return; // Jetzt gehts weiter in SIGNAL (SIG_SPI)
70	/*	70	/*
71	case MM3_TILT: // Zeitnahe Speicherung der aktuellen Neigung in °	71	case MM3_TILT: // Zeitnahe Speicherung der aktuellen Neigung in °
72	MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8);	72	MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8);
73	MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8);	73	MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8);
74		74
75	MM3.AXIS = MM3_X;	75	MM3.AXIS = MM3_X;
76	MM3.STATE = MM3_RESET;	76	MM3.STATE = MM3_RESET;
77	return;	77	return;
78	*/	78	*/
79	}	79	}
80	}	80	}
81		81
82		82
83	//############################################################################	83	//############################################################################
84	// SPI byte ready	84	// SPI byte ready
85	SIGNAL (SIG_SPI)	85	SIGNAL (SIG_SPI)
86	//############################################################################	86	//############################################################################
87	{	87	{
88	switch (MM3.STATE)	88	switch (MM3.STATE)
89	{	89	{
90	case MM3_DRDY: // 1. Byte ist da, abspeichern, an die MSB-Stelle rücken	90	case MM3_DRDY: // 1. Byte ist da, abspeichern, an die MSB-Stelle rücken
91	if (MM3.AXIS == MM3_X)	91	if (MM3.AXIS == MM3_X)
92	{	92	{
93	MM3.x_axis = SPDR;	93	MM3.x_axis = SPDR;
94	MM3.x_axis <<= 8;	94	MM3.x_axis <<= 8;
95	}	95	}
96	else if (MM3.AXIS == MM3_Y)	96	else if (MM3.AXIS == MM3_Y)
97	{	97	{
98	MM3.y_axis = SPDR;	98	MM3.y_axis = SPDR;
99	MM3.y_axis <<= 8;	99	MM3.y_axis <<= 8;
100	}	100	}
101	else // if (MM3.AXIS == MM3_Z)	101	else // if (MM3.AXIS == MM3_Z)
102	{	102	{
103	MM3.z_axis = SPDR;	103	MM3.z_axis = SPDR;
104	MM3.z_axis <<= 8;	104	MM3.z_axis <<= 8;
105	}	105	}
106		106
107	SPDR=0x00; // Übertragung von 2. Byte auslösen	107	SPDR=0x00; // Übertragung von 2. Byte auslösen
108	MM3.STATE=MM3_BYTE2;	108	MM3.STATE=MM3_BYTE2;
109	return;	109	return;
110		110
111	case MM3_BYTE2: // 2. Byte der entsprechenden Achse ist da	111	case MM3_BYTE2: // 2. Byte der entsprechenden Achse ist da
112	if (MM3.AXIS == MM3_X)	112	if (MM3.AXIS == MM3_X)
113	{	113	{
114	MM3.x_axis \|= SPDR;	114	MM3.x_axis \|= SPDR;
115	// Spikes filtern	115	// Spikes filtern
116	if (abs(MM3.x_axis) < Max_Axis_Value) MM3.x_axis_old = MM3.x_axis;	116	if (abs(MM3.x_axis) < Max_Axis_Value) MM3.x_axis_old = MM3.x_axis;
117	else MM3.x_axis = MM3.x_axis_old;	117	else MM3.x_axis = MM3.x_axis_old;
118	MM3.AXIS = MM3_Y;	118	MM3.AXIS = MM3_Y;
119	MM3.STATE = MM3_RESET;	119	MM3.STATE = MM3_RESET;
120	}	120	}
121	else if (MM3.AXIS == MM3_Y)	121	else if (MM3.AXIS == MM3_Y)
122	{	122	{
123	MM3.y_axis \|= SPDR;	123	MM3.y_axis \|= SPDR;
124	if (abs(MM3.y_axis) < Max_Axis_Value) MM3.y_axis_old = MM3.y_axis;	124	if (abs(MM3.y_axis) < Max_Axis_Value) MM3.y_axis_old = MM3.y_axis;
125	else MM3.y_axis = MM3.y_axis_old;	125	else MM3.y_axis = MM3.y_axis_old;
126	MM3.AXIS = MM3_Z;	126	MM3.AXIS = MM3_Z;
127	MM3.STATE = MM3_RESET;	127	MM3.STATE = MM3_RESET;
128	}	128	}
129	else // if (MM3.AXIS == MM3_Z)	129	else // if (MM3.AXIS == MM3_Z)
130	{	130	{
131	MM3.z_axis \|= SPDR;	131	MM3.z_axis \|= SPDR;
132	if (abs(MM3.z_axis) < Max_Axis_Value) MM3.z_axis_old = MM3.z_axis;	132	if (abs(MM3.z_axis) < Max_Axis_Value) MM3.z_axis_old = MM3.z_axis;
133	else MM3.z_axis = MM3.z_axis_old;	133	else MM3.z_axis = MM3.z_axis_old;
134	MM3.AXIS = MM3_X;	134	MM3.AXIS = MM3_X;
135	MM3.STATE = MM3_RESET;	135	MM3.STATE = MM3_RESET;
136	}	136	}
137		137
138	return;	138	return;
139	}	139	}
140	}	140	}
141		141
142	//############################################################################	142	//############################################################################
143	// Kompass kalibrieren	143	// Kompass kalibrieren
144	void calib_MM3(void)	144	void calib_MM3(void)
145	//############################################################################	145	//############################################################################
146	{	146	{
147	signed int x_min=0,x_max=0,y_min=0,y_max=0,z_min=0,z_max=0;	147	signed int x_min=0,x_max=0,y_min=0,y_max=0,z_min=0,z_max=0;
148	uint8_t measurement=50,beeper=0;	148	uint8_t measurement=50,beeper=0;
149	unsigned int timer;	149	unsigned int timer;
150		150
151	GRN_ON;	151	GRN_ON;
152	ROT_OFF;	152	ROT_OFF;
153		153
154	while (measurement)	154	while (measurement)
155	{	155	{
156	//H_earth = MM3.x_axisMM3.x_axis + MM3.y_axisMM3.y_axis + MM3.z_axis*MM3.z_axis;	156	//H_earth = MM3.x_axisMM3.x_axis + MM3.y_axisMM3.y_axis + MM3.z_axis*MM3.z_axis;
157		157
158	if (MM3.x_axis > x_max) x_max = MM3.x_axis;	158	if (MM3.x_axis > x_max) x_max = MM3.x_axis;
159	else if (MM3.x_axis < x_min) x_min = MM3.x_axis;	159	else if (MM3.x_axis < x_min) x_min = MM3.x_axis;
160		160
161	if (MM3.y_axis > y_max) y_max = MM3.y_axis;	161	if (MM3.y_axis > y_max) y_max = MM3.y_axis;
162	else if (MM3.y_axis < y_min) y_min = MM3.y_axis;	162	else if (MM3.y_axis < y_min) y_min = MM3.y_axis;
163		163
164	if (MM3.z_axis > z_max) z_max = MM3.z_axis;	164	if (MM3.z_axis > z_max) z_max = MM3.z_axis;
165	else if (MM3.z_axis < z_min) z_min = MM3.z_axis;	165	else if (MM3.z_axis < z_min) z_min = MM3.z_axis;
166		166
167	if (!beeper)	167	if (!beeper)
168	{	168	{
169	ROT_FLASH;	169	ROT_FLASH;
170	GRN_FLASH;	170	GRN_FLASH;
-		171	//beeptime = 50;
171	beeper = 50;	172	beeper = 50;
172	}	173	}
173	beeper--;	174	beeper--;
174		175
175	// Schleife mit 100 Hz voll ausreichend	176	// Schleife mit 100 Hz
176	timer = SetDelay(10);	177	timer = SetDelay(10);
177	while(!CheckDelay(timer));	178	while(!CheckDelay(timer));
178		179
179	// Wenn Gas zurück genommen wird, Kalibrierung mit Verzögerung beenden	180	// Wenn Gas zurück genommen wird, Kalibrierung mit 1/2 Sekunde Verzögerung beenden
180	if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--;	181	if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--;
181	}	182	}
182		183
183	// Offset der Achsen berechnen	184	// Offset der Achsen berechnen
184	MM3_calib.X_off = (x_max + x_min) / 2;	185	MM3_calib.X_off = (x_max + x_min) / 2;
185	MM3_calib.Y_off = (y_max + y_min) / 2;	186	MM3_calib.Y_off = (y_max + y_min) / 2;
186	MM3_calib.Z_off = (z_max + z_min) / 2;	187	MM3_calib.Z_off = (z_max + z_min) / 2;
187		188
188	// und im EEProm abspeichern	189	// und im EEProm abspeichern
189	eeprom_write_block(&MM3_calib,&ee_calib,sizeof(MM3_calib_struct));	190	eeprom_write_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct));
190		191
191	}	192	}
192		193
193		194
194	//############################################################################	195	//############################################################################
195	// Neigungskompensierung und Berechnung der Ausrichtung	196	// Neigungskompensierung und Berechnung der Ausrichtung
196	signed int heading_MM3(void)	197	signed int heading_MM3(void)
197	//############################################################################	198	//############################################################################
198	{	199	{
199	float sin_nick, cos_nick, sin_roll, cos_roll;	200	float sin_nick, cos_nick, sin_roll, cos_roll;
200	signed int x_corr, y_corr, heading;	201	signed int x_corr, y_corr, heading;
201	signed int x_axis,y_axis,z_axis;	202	signed int x_axis,y_axis,z_axis;
202		203
203	MM3.NickGrad = -(IntegralNick/(EE_Parameter.UserParam1*8));	204	MM3.NickGrad = -(IntegralNick/(EE_Parameter.UserParam1*8));
204	MM3.RollGrad = -(IntegralRoll/(EE_Parameter.UserParam2*8));	205	MM3.RollGrad = -(IntegralRoll/(EE_Parameter.UserParam2*8));
205		206
206	// Berechung von sinus und cosinus	207	// Berechung von sinus und cosinus
207	sin_nick = sin_f(MM3.NickGrad);	208	sin_nick = sin_f(MM3.NickGrad);
208	cos_nick = cos_f(MM3.NickGrad);	209	cos_nick = cos_f(MM3.NickGrad);
209	sin_roll = sin_f(MM3.RollGrad);	210	sin_roll = sin_f(MM3.RollGrad);
210	cos_roll = cos_f(MM3.RollGrad);	211	cos_roll = cos_f(MM3.RollGrad);
211		212
212	// Offset der Achsen nur bei Bedarf (also hier) berücksichtigen	213	// Offset der Achsen nur bei Bedarf (also hier) berücksichtigen
213	x_axis = -(MM3.x_axis - MM3_calib.X_off);	214	x_axis = -(MM3.x_axis - MM3_calib.X_off);
214	y_axis = -(MM3.y_axis - MM3_calib.Y_off);	215	y_axis = -(MM3.y_axis - MM3_calib.Y_off);
215	z_axis = -(MM3.z_axis - MM3_calib.Z_off);	216	z_axis = -(MM3.z_axis - MM3_calib.Z_off);
216		217
217	// Neigungskompensation	218	// Neigungskompensation
218	x_corr = (cos_nick * x_axis) + (((sin_roll * y_axis) - (cos_roll * z_axis)) * sin_nick);	219	x_corr = (cos_nick * x_axis) + (((sin_roll * y_axis) - (cos_roll * z_axis)) * sin_nick);
219	y_corr = ((cos_roll * y_axis) + (sin_roll * z_axis));	220	y_corr = ((cos_roll * y_axis) + (sin_roll * z_axis));
220		221
221	// Winkelberechnung	222	// Winkelberechnung
222	heading = atan2_i(x_corr, y_corr);	223	heading = atan2_i(x_corr, y_corr);
223		224
224	return (heading);	225	return (heading);
225	}	226	}
226		227

Subversion Repositories FlightCtrl

(root)/branches/v0.60_MicroMag3_Nick666/trunc/compass.c @ 373 - Rev 371 → 373