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

Subversion Repositories FlightCtrl

(root)/branches/v0.60_MicroMag3_Nick666/trunc/compass.c @ 422 - Rev 409 → 412