WebSVN - FlightCtrl - Diff - Rev 554 and 557 - /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;         // Reservierung im EEPROM
 
struct MM3_working_struct MM3;
struct MM3_calib_struct MM3_calib;
 
 
//############################################################################
// Initialisierung
void init_MM3(void)
//############################################################################
{
        // SPI-Schnittstelle initialisieren
        SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0);        // Interrupt an, Master, 156 kHz Oszillator
 
    DDRB |= (1<<PB7)|(1<<PB5)|(1<<PB2); // J8, MOSI, SCK Ausgang
       
        if(PlatinenVersion == 10)
        {
                DDRD |= (1<<PD3);               // PD3 als Ausgang
                PORTD &= ~(1<<PD3);     // J5 permanent auf Low
        }
        else
        {
                DDRC |= (1<<PC6);               // PC6 als Ausgang
                PORTC &= ~(1<<PC6);     // J9 permanent auf Low
        }
       
        // Init Statemachine
        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 SPI-Übertragung (MOSI und MISO) aus
                else if (MM3.AXIS == MM3_Y) SPDR = 0x32;                // Micromag Period Select ist auf 256 (0x30)
                else SPDR = 0x33;       //if (MM3.AXIS == MM3_Z)        // 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)
        }
}
 
 
//############################################################################
// SPI byte ready
SIGNAL (SIG_SPI)
//############################################################################
{
        static char tmp;
        int wert;
 
        switch (MM3.STATE)
        {
        case MM3_DRDY:          // 1. Byte ist da, zwischenspeichern
                tmp = SPDR;
                SPDR = 0x00;    // Übertragung von 2. Byte auslösen
                MM3.STATE = MM3_BYTE2;
                return;
               
        case MM3_BYTE2:         // 2. Byte der entsprechenden Achse ist da
                wert = tmp;
                wert <<= 8;             // 1. Byte an MSB-Stelle rücken
                wert |= SPDR;   // 2. Byte dranpappen
               
                if(abs(wert) < Max_Axis_Value)          // Spikes filtern. Zuweisung nur, wenn Max-Wert nicht überschritten
                switch (MM3.AXIS)
                {
                case MM3_X:
                        MM3.x_axis = wert;
                        MM3.AXIS = MM3_Y;
                        break;
                case MM3_Y:
                        MM3.y_axis = wert;
                        MM3.AXIS = MM3_Z;
                        break;
                default:        //case MM3_Z:
                        MM3.z_axis = wert;
                        MM3.AXIS = MM3_X;
                }
               
                MM3.STATE = MM3_RESET;
        }
}
 
//############################################################################
// 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--;
        }
       
        // Wertebereich der Achsen
        MM3_calib.X_range = (x_max - x_min);
        MM3_calib.Y_range = (y_max - y_min);
        MM3_calib.Z_range = (z_max - z_min);
       
        // Offset der Achsen
        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;
        float x_corr, y_corr;
        signed int x_axis,y_axis,z_axis,heading;
        signed int nicktilt,rolltilt;
        unsigned int div_faktor;
               
        div_faktor = (uint16_t)EE_Parameter.UserParam3 * 8;
       
        // Berechung von sinus und cosinus
        nicktilt = (IntegralNick/div_faktor);
        sin_nick = sin_f(nicktilt);
        cos_nick = cos_f(nicktilt);
       
        rolltilt = (IntegralRoll/div_faktor);
        sin_roll = sin_f(rolltilt);
        cos_roll = cos_f(rolltilt);
       
        // Offset
        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);
 
        // Normierung Wertebereich
        if ((MM3_calib.X_range > MM3_calib.Y_range) && (MM3_calib.X_range > MM3_calib.Z_range))
        {
                y_axis = ((long)y_axis * MM3_calib.X_range) / MM3_calib.Y_range;
                z_axis = ((long)z_axis * MM3_calib.X_range) / MM3_calib.Z_range;
        }
        else if ((MM3_calib.Y_range > MM3_calib.X_range) && (MM3_calib.Y_range > MM3_calib.Z_range))
        {
                x_axis = ((long)x_axis * MM3_calib.Y_range) / MM3_calib.X_range;
                z_axis = ((long)z_axis * MM3_calib.Y_range) / MM3_calib.Z_range;
        }
        else //if ((MM3_calib.Z_range > MM3_calib.X_range) && (MM3_calib.Z_range > MM3_calib.Y_range))
        {
                x_axis = ((long)x_axis * MM3_calib.Z_range) / MM3_calib.X_range;
                y_axis = ((long)y_axis * MM3_calib.Z_range) / MM3_calib.Y_range;
        }
 
    // Neigungskompensation
        x_corr = x_axis * cos_nick;
        x_corr += y_axis * sin_roll * sin_nick;
        x_corr -= z_axis * cos_roll * sin_nick;
       
        y_corr = y_axis * cos_roll;
        y_corr += z_axis * sin_roll;
       
        // Winkelberechnung     
        heading = atan2_i(x_corr, y_corr);
        if (heading < 0) heading = -heading;
        else heading = 360 - heading;  
       
        /*
        if (!x_corr && y_corr <0) return (90);
        if (!x_corr && y_corr >0) return (270);
       
        heading = atan(y_corr/x_corr)*57.29578;
        if (x_corr < 0) heading = 180-heading;
        if (x_corr > 0 && y_corr < 0) heading = -heading;
        if (x_corr > 0 && y_corr > 0) heading = 360 - heading;
        */
return (heading);
}
 

Rev 554	Rev 557
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	struct MM3_calib_struct ee_calib EEMEM; // Reservierung im EEPROM	18	struct MM3_calib_struct ee_calib EEMEM; // Reservierung im EEPROM
19		19
20	struct MM3_working_struct MM3;	20	struct MM3_working_struct MM3;
21	struct 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	// SPI-Schnittstelle initialisieren	29	// SPI-Schnittstelle initialisieren
30	SPCR = (1<<SPIE)\|(1<<SPE)\|(1<<MSTR)\|(1<<SPR1)\|(1<<SPR0); // Interrupt an, Master, 156 kHz Oszillator	30	SPCR = (1<<SPIE)\|(1<<SPE)\|(1<<MSTR)\|(1<<SPR1)\|(1<<SPR0); // Interrupt an, Master, 156 kHz Oszillator
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
-		34	if(PlatinenVersion == 10)
-		35	{
33		36	DDRD \|= (1<<PD3); // PD3 als Ausgang
-		37	PORTD &= ~(1<<PD3); // J5 permanent auf Low
-		38	}
-		39	else
-		40	{
-		41	DDRC \|= (1<<PC6); // PC6 als Ausgang
-		42	PORTC &= ~(1<<PC6); // J9 permanent auf Low
34	PORTD &= ~(1<<PD3); // J5 permanent auf Low	43	}
35		44
36	// Init Statemachine	45	// Init Statemachine
37	MM3.AXIS = MM3_X;	46	MM3.AXIS = MM3_X;
38	MM3.STATE = MM3_RESET;	47	MM3.STATE = MM3_RESET;
39		48
40	// Kalibrierung aus dem EEprom lesen	49	// Kalibrierung aus dem EEprom lesen
41	eeprom_read_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct));	50	eeprom_read_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct));
42	}	51	}
43		52
44		53
45	//############################################################################	54	//############################################################################
46	// Wird in der SIGNAL (SIG_OVERFLOW0) aufgerufen	55	// Wird in der SIGNAL (SIG_OVERFLOW0) aufgerufen
47	void timer0_MM3(void)	56	void timer0_MM3(void)
48	//############################################################################	57	//############################################################################
49	{	58	{
50	switch (MM3.STATE)	59	switch (MM3.STATE)
51	{	60	{
52	case MM3_RESET:	61	case MM3_RESET:
53	PORTB \|= (1<<PB2); // J8 auf High, MM3 Reset	62	PORTB \|= (1<<PB2); // J8 auf High, MM3 Reset
54	MM3.STATE = MM3_START_TRANSFER;	63	MM3.STATE = MM3_START_TRANSFER;
55	return;	64	return;
56		65
57	case MM3_START_TRANSFER:	66	case MM3_START_TRANSFER:
58	PORTB &= ~(1<<PB2); // J8 auf Low (war ~125 µs auf High)	67	PORTB &= ~(1<<PB2); // J8 auf Low (war ~125 µs auf High)
59		68
60	if (MM3.AXIS == MM3_X) SPDR = 0x31; // Schreiben ins SPDR löst automatisch SPI-Übertragung (MOSI und MISO) aus	69	if (MM3.AXIS == MM3_X) SPDR = 0x31; // Schreiben ins SPDR löst automatisch SPI-Übertragung (MOSI und MISO) aus
61	else if (MM3.AXIS == MM3_Y) SPDR = 0x32; // Micromag Period Select ist auf 256 (0x30)	70	else if (MM3.AXIS == MM3_Y) SPDR = 0x32; // Micromag Period Select ist auf 256 (0x30)
62	else SPDR = 0x33; //if (MM3.AXIS == MM3_Z) // 1: x-Achse, 2: Y-Achse, 3: Z-Achse	71	else SPDR = 0x33; //if (MM3.AXIS == MM3_Z) // 1: x-Achse, 2: Y-Achse, 3: Z-Achse
63		72
64	MM3.DRDY = SetDelay(8); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms)	73	MM3.DRDY = SetDelay(8); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms)
65	MM3.STATE = MM3_WAIT_DRDY;	74	MM3.STATE = MM3_WAIT_DRDY;
66	return;	75	return;
67		76
68	case MM3_WAIT_DRDY:	77	case MM3_WAIT_DRDY:
69	if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei	78	if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei
70	return; // Jetzt gehts weiter in SIGNAL (SIG_SPI)	79	return; // Jetzt gehts weiter in SIGNAL (SIG_SPI)
71	}	80	}
72	}	81	}
73		82
74		83
75	//############################################################################	84	//############################################################################
76	// SPI byte ready	85	// SPI byte ready
77	SIGNAL (SIG_SPI)	86	SIGNAL (SIG_SPI)
78	//############################################################################	87	//############################################################################
79	{	88	{
80	static char tmp;	89	static char tmp;
81	int wert;	90	int wert;
82		91
83	switch (MM3.STATE)	92	switch (MM3.STATE)
84	{	93	{
85	case MM3_DRDY: // 1. Byte ist da, zwischenspeichern	94	case MM3_DRDY: // 1. Byte ist da, zwischenspeichern
86	tmp = SPDR;	95	tmp = SPDR;
87	SPDR = 0x00; // Übertragung von 2. Byte auslösen	96	SPDR = 0x00; // Übertragung von 2. Byte auslösen
88	MM3.STATE = MM3_BYTE2;	97	MM3.STATE = MM3_BYTE2;
89	return;	98	return;
90		99
91	case MM3_BYTE2: // 2. Byte der entsprechenden Achse ist da	100	case MM3_BYTE2: // 2. Byte der entsprechenden Achse ist da
92	wert = tmp;	101	wert = tmp;
93	wert <<= 8; // 1. Byte an MSB-Stelle rücken	102	wert <<= 8; // 1. Byte an MSB-Stelle rücken
94	wert \|= SPDR; // 2. Byte dranpappen	103	wert \|= SPDR; // 2. Byte dranpappen
95		104
96	if(abs(wert) < Max_Axis_Value) // Spikes filtern. Zuweisung nur, wenn Max-Wert nicht überschritten	105	if(abs(wert) < Max_Axis_Value) // Spikes filtern. Zuweisung nur, wenn Max-Wert nicht überschritten
97	switch (MM3.AXIS)	106	switch (MM3.AXIS)
98	{	107	{
99	case MM3_X:	108	case MM3_X:
100	MM3.x_axis = wert;	109	MM3.x_axis = wert;
101	MM3.AXIS = MM3_Y;	110	MM3.AXIS = MM3_Y;
102	break;	111	break;
103	case MM3_Y:	112	case MM3_Y:
104	MM3.y_axis = wert;	113	MM3.y_axis = wert;
105	MM3.AXIS = MM3_Z;	114	MM3.AXIS = MM3_Z;
106	break;	115	break;
107	default: //case MM3_Z:	116	default: //case MM3_Z:
108	MM3.z_axis = wert;	117	MM3.z_axis = wert;
109	MM3.AXIS = MM3_X;	118	MM3.AXIS = MM3_X;
110	}	119	}
111		120
112	MM3.STATE = MM3_RESET;	121	MM3.STATE = MM3_RESET;
113	}	122	}
114	}	123	}
115		124
116	//############################################################################	125	//############################################################################
117	// Kompass kalibrieren	126	// Kompass kalibrieren
118	void calib_MM3(void)	127	void calib_MM3(void)
119	//############################################################################	128	//############################################################################
120	{	129	{
121	signed int x_min=0,x_max=0,y_min=0,y_max=0,z_min=0,z_max=0;	130	signed int x_min=0,x_max=0,y_min=0,y_max=0,z_min=0,z_max=0;
122	uint8_t measurement=50,beeper=0;	131	uint8_t measurement=50,beeper=0;
123	unsigned int timer;	132	unsigned int timer;
124		133
125	GRN_ON;	134	GRN_ON;
126	ROT_OFF;	135	ROT_OFF;
127		136
128	while (measurement)	137	while (measurement)
129	{	138	{
130	//H_earth = MM3.x_axisMM3.x_axis + MM3.y_axisMM3.y_axis + MM3.z_axis*MM3.z_axis;	139	//H_earth = MM3.x_axisMM3.x_axis + MM3.y_axisMM3.y_axis + MM3.z_axis*MM3.z_axis;
131		140
132	if (MM3.x_axis > x_max) x_max = MM3.x_axis;	141	if (MM3.x_axis > x_max) x_max = MM3.x_axis;
133	else if (MM3.x_axis < x_min) x_min = MM3.x_axis;	142	else if (MM3.x_axis < x_min) x_min = MM3.x_axis;
134		143
135	if (MM3.y_axis > y_max) y_max = MM3.y_axis;	144	if (MM3.y_axis > y_max) y_max = MM3.y_axis;
136	else if (MM3.y_axis < y_min) y_min = MM3.y_axis;	145	else if (MM3.y_axis < y_min) y_min = MM3.y_axis;
137		146
138	if (MM3.z_axis > z_max) z_max = MM3.z_axis;	147	if (MM3.z_axis > z_max) z_max = MM3.z_axis;
139	else if (MM3.z_axis < z_min) z_min = MM3.z_axis;	148	else if (MM3.z_axis < z_min) z_min = MM3.z_axis;
140		149
141	if (!beeper)	150	if (!beeper)
142	{	151	{
143	ROT_FLASH;	152	ROT_FLASH;
144	GRN_FLASH;	153	GRN_FLASH;
145	beeptime = 50;	154	beeptime = 50;
146	beeper = 50;	155	beeper = 50;
147	}	156	}
148	beeper--;	157	beeper--;
149		158
150	// Schleife mit 100 Hz	159	// Schleife mit 100 Hz
151	timer = SetDelay(10);	160	timer = SetDelay(10);
152	while(!CheckDelay(timer));	161	while(!CheckDelay(timer));
153		162
154	// Wenn Gas zurück genommen wird, Kalibrierung mit 1/2 Sekunde Verzögerung beenden	163	// Wenn Gas zurück genommen wird, Kalibrierung mit 1/2 Sekunde Verzögerung beenden
155	if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--;	164	if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--;
156	}	165	}
157		166
158	// Wertebereich der Achsen	167	// Wertebereich der Achsen
159	MM3_calib.X_range = (x_max - x_min);	168	MM3_calib.X_range = (x_max - x_min);
160	MM3_calib.Y_range = (y_max - y_min);	169	MM3_calib.Y_range = (y_max - y_min);
161	MM3_calib.Z_range = (z_max - z_min);	170	MM3_calib.Z_range = (z_max - z_min);
162		171
163	// Offset der Achsen	172	// Offset der Achsen
164	MM3_calib.X_off = (x_max + x_min) / 2;	173	MM3_calib.X_off = (x_max + x_min) / 2;
165	MM3_calib.Y_off = (y_max + y_min) / 2;	174	MM3_calib.Y_off = (y_max + y_min) / 2;
166	MM3_calib.Z_off = (z_max + z_min) / 2;	175	MM3_calib.Z_off = (z_max + z_min) / 2;
167		176
168	// und im EEProm abspeichern	177	// und im EEProm abspeichern
169	eeprom_write_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct));	178	eeprom_write_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct));
170	}	179	}
171		180
172		181
173	//############################################################################	182	//############################################################################
174	// Neigungskompensierung und Berechnung der Ausrichtung	183	// Neigungskompensierung und Berechnung der Ausrichtung
175	signed int heading_MM3(void)	184	signed int heading_MM3(void)
176	//############################################################################	185	//############################################################################
177	{	186	{
178	float sin_nick, cos_nick, sin_roll, cos_roll;	187	float sin_nick, cos_nick, sin_roll, cos_roll;
179	float x_corr, y_corr;	188	float x_corr, y_corr;
180	signed int x_axis,y_axis,z_axis,heading;	189	signed int x_axis,y_axis,z_axis,heading;
181	signed int nicktilt,rolltilt;	190	signed int nicktilt,rolltilt;
182	unsigned int div_faktor;	191	unsigned int div_faktor;
183		192
184	div_faktor = (uint16_t)EE_Parameter.UserParam3 * 8;	193	div_faktor = (uint16_t)EE_Parameter.UserParam3 * 8;
185		194
186	// Berechung von sinus und cosinus	195	// Berechung von sinus und cosinus
187	nicktilt = (IntegralNick/div_faktor);	196	nicktilt = (IntegralNick/div_faktor);
188	sin_nick = sin_f(nicktilt);	197	sin_nick = sin_f(nicktilt);
189	cos_nick = cos_f(nicktilt);	198	cos_nick = cos_f(nicktilt);
190		199
191	rolltilt = (IntegralRoll/div_faktor);	200	rolltilt = (IntegralRoll/div_faktor);
192	sin_roll = sin_f(rolltilt);	201	sin_roll = sin_f(rolltilt);
193	cos_roll = cos_f(rolltilt);	202	cos_roll = cos_f(rolltilt);
194		203
195	// Offset	204	// Offset
196	x_axis = (MM3.x_axis - MM3_calib.X_off);	205	x_axis = (MM3.x_axis - MM3_calib.X_off);
197	y_axis = (MM3.y_axis - MM3_calib.Y_off);	206	y_axis = (MM3.y_axis - MM3_calib.Y_off);
198	z_axis = (MM3.z_axis - MM3_calib.Z_off);	207	z_axis = (MM3.z_axis - MM3_calib.Z_off);
199		208
200	// Normierung Wertebereich	209	// Normierung Wertebereich
201	if ((MM3_calib.X_range > MM3_calib.Y_range) && (MM3_calib.X_range > MM3_calib.Z_range))	210	if ((MM3_calib.X_range > MM3_calib.Y_range) && (MM3_calib.X_range > MM3_calib.Z_range))
202	{	211	{
203	y_axis = ((long)y_axis * MM3_calib.X_range) / MM3_calib.Y_range;	212	y_axis = ((long)y_axis * MM3_calib.X_range) / MM3_calib.Y_range;
204	z_axis = ((long)z_axis * MM3_calib.X_range) / MM3_calib.Z_range;	213	z_axis = ((long)z_axis * MM3_calib.X_range) / MM3_calib.Z_range;
205	}	214	}
206	else if ((MM3_calib.Y_range > MM3_calib.X_range) && (MM3_calib.Y_range > MM3_calib.Z_range))	215	else if ((MM3_calib.Y_range > MM3_calib.X_range) && (MM3_calib.Y_range > MM3_calib.Z_range))
207	{	216	{
208	x_axis = ((long)x_axis * MM3_calib.Y_range) / MM3_calib.X_range;	217	x_axis = ((long)x_axis * MM3_calib.Y_range) / MM3_calib.X_range;
209	z_axis = ((long)z_axis * MM3_calib.Y_range) / MM3_calib.Z_range;	218	z_axis = ((long)z_axis * MM3_calib.Y_range) / MM3_calib.Z_range;
210	}	219	}
211	else //if ((MM3_calib.Z_range > MM3_calib.X_range) && (MM3_calib.Z_range > MM3_calib.Y_range))	220	else //if ((MM3_calib.Z_range > MM3_calib.X_range) && (MM3_calib.Z_range > MM3_calib.Y_range))
212	{	221	{
213	x_axis = ((long)x_axis * MM3_calib.Z_range) / MM3_calib.X_range;	222	x_axis = ((long)x_axis * MM3_calib.Z_range) / MM3_calib.X_range;
214	y_axis = ((long)y_axis * MM3_calib.Z_range) / MM3_calib.Y_range;	223	y_axis = ((long)y_axis * MM3_calib.Z_range) / MM3_calib.Y_range;
215	}	224	}
216		225
217	// Neigungskompensation	226	// Neigungskompensation
218	x_corr = x_axis * cos_nick;	227	x_corr = x_axis * cos_nick;
219	x_corr += y_axis * sin_roll * sin_nick;	228	x_corr += y_axis * sin_roll * sin_nick;
220	x_corr -= z_axis * cos_roll * sin_nick;	229	x_corr -= z_axis * cos_roll * sin_nick;
221		230
222	y_corr = y_axis * cos_roll;	231	y_corr = y_axis * cos_roll;
223	y_corr += z_axis * sin_roll;	232	y_corr += z_axis * sin_roll;
224		233
225	// Winkelberechnung	234	// Winkelberechnung
226	heading = atan2_i(x_corr, y_corr);	235	heading = atan2_i(x_corr, y_corr);
227	if (heading < 0) heading = -heading;	236	if (heading < 0) heading = -heading;
228	else heading = 360 - heading;	237	else heading = 360 - heading;
229		238
230	/*	239	/*
231	if (!x_corr && y_corr <0) return (90);	240	if (!x_corr && y_corr <0) return (90);
232	if (!x_corr && y_corr >0) return (270);	241	if (!x_corr && y_corr >0) return (270);
233		242
234	heading = atan(y_corr/x_corr)*57.29578;	243	heading = atan(y_corr/x_corr)*57.29578;
235	if (x_corr < 0) heading = 180-heading;	244	if (x_corr < 0) heading = 180-heading;
236	if (x_corr > 0 && y_corr < 0) heading = -heading;	245	if (x_corr > 0 && y_corr < 0) heading = -heading;
237	if (x_corr > 0 && y_corr > 0) heading = 360 - heading;	246	if (x_corr > 0 && y_corr > 0) heading = 360 - heading;
238	*/	247	*/
239	return (heading);	248	return (heading);
240	}	249	}
241		250

Subversion Repositories FlightCtrl

(root)/branches/v0.60_MicroMag3_Nick666/V0.67g/compass.c - Rev 554 → 557