WebSVN - FlightCtrl - Diff - Rev 378 and 386 - /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)
//############################################################################
{      
        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--;
        }
       
        // 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 = (MM3_calib.X_range / 2) - x_max;
        MM3_calib.Y_off = (MM3_calib.Y_range / 2) - y_max;
        MM3_calib.Z_off = (MM3_calib.Z_range / 2) - z_max;
 
        // 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);
        sin_nick = sin_f(MM3.NickGrad);
        cos_nick = cos_f(MM3.NickGrad);
       
        MM3.RollGrad = -(IntegralRoll/div_faktor);
        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 (!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 378	Rev 386
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		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(struct 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(5); // 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)
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	}	71	}
72		72
73		73
74	//############################################################################	74	//############################################################################
75	// SPI byte ready	75	// SPI byte ready
76	SIGNAL (SIG_SPI)	76	SIGNAL (SIG_SPI)
77	//############################################################################	77	//############################################################################
78	{	78	{
79	switch (MM3.STATE)	79	switch (MM3.STATE)
80	{	80	{
81	case MM3_DRDY: // 1. Byte ist da, abspeichern, an die MSB-Stelle rücken	81	case MM3_DRDY: // 1. Byte ist da, abspeichern, an die MSB-Stelle rücken
82	if (MM3.AXIS == MM3_X)	82	if (MM3.AXIS == MM3_X)
83	{	83	{
84	MM3.x_axis = SPDR;	84	MM3.x_axis = SPDR;
85	MM3.x_axis <<= 8;	85	MM3.x_axis <<= 8;
86	}	86	}
87	else if (MM3.AXIS == MM3_Y)	87	else if (MM3.AXIS == MM3_Y)
88	{	88	{
89	MM3.y_axis = SPDR;	89	MM3.y_axis = SPDR;
90	MM3.y_axis <<= 8;	90	MM3.y_axis <<= 8;
91	}	91	}
92	else // if (MM3.AXIS == MM3_Z)	92	else // if (MM3.AXIS == MM3_Z)
93	{	93	{
94	MM3.z_axis = SPDR;	94	MM3.z_axis = SPDR;
95	MM3.z_axis <<= 8;	95	MM3.z_axis <<= 8;
96	}	96	}
97		97
98	SPDR=0x00; // Übertragung von 2. Byte auslösen	98	SPDR=0x00; // Übertragung von 2. Byte auslösen
99	MM3.STATE=MM3_BYTE2;	99	MM3.STATE=MM3_BYTE2;
100	return;	100	return;
101		101
102	case MM3_BYTE2: // 2. Byte der entsprechenden Achse ist da	102	case MM3_BYTE2: // 2. Byte der entsprechenden Achse ist da
103	if (MM3.AXIS == MM3_X)	103	if (MM3.AXIS == MM3_X)
104	{	104	{
105	MM3.x_axis \|= SPDR;	105	MM3.x_axis \|= SPDR;
106	// Spikes filtern	106	// Spikes filtern
107	if (abs(MM3.x_axis) < Max_Axis_Value) MM3.x_axis_old = MM3.x_axis;	107	if (abs(MM3.x_axis) < Max_Axis_Value) MM3.x_axis_old = MM3.x_axis;
108	else MM3.x_axis = MM3.x_axis_old;	108	else MM3.x_axis = MM3.x_axis_old;
109	MM3.AXIS = MM3_Y;	109	MM3.AXIS = MM3_Y;
110	MM3.STATE = MM3_RESET;	110	MM3.STATE = MM3_RESET;
111	}	111	}
112	else if (MM3.AXIS == MM3_Y)	112	else if (MM3.AXIS == MM3_Y)
113	{	113	{
114	MM3.y_axis \|= SPDR;	114	MM3.y_axis \|= SPDR;
115	if (abs(MM3.y_axis) < Max_Axis_Value) MM3.y_axis_old = MM3.y_axis;	115	if (abs(MM3.y_axis) < Max_Axis_Value) MM3.y_axis_old = MM3.y_axis;
116	else MM3.y_axis = MM3.y_axis_old;	116	else MM3.y_axis = MM3.y_axis_old;
117	MM3.AXIS = MM3_Z;	117	MM3.AXIS = MM3_Z;
118	MM3.STATE = MM3_RESET;	118	MM3.STATE = MM3_RESET;
119	}	119	}
120	else // if (MM3.AXIS == MM3_Z)	120	else // if (MM3.AXIS == MM3_Z)
121	{	121	{
122	MM3.z_axis \|= SPDR;	122	MM3.z_axis \|= SPDR;
123	if (abs(MM3.z_axis) < Max_Axis_Value) MM3.z_axis_old = MM3.z_axis;	123	if (abs(MM3.z_axis) < Max_Axis_Value) MM3.z_axis_old = MM3.z_axis;
124	else MM3.z_axis = MM3.z_axis_old;	124	else MM3.z_axis = MM3.z_axis_old;
125	MM3.AXIS = MM3_X;	125	MM3.AXIS = MM3_X;
126	MM3.STATE = MM3_RESET;	126	MM3.STATE = MM3_RESET;
127	}	127	}
128		128
129	return;	129	return;
130	}	130	}
131	}	131	}
132		132
133	//############################################################################	133	//############################################################################
134	// Kompass kalibrieren	134	// Kompass kalibrieren
135	void calib_MM3(void)	135	void calib_MM3(void)
136	//############################################################################	136	//############################################################################
137	{	137	{
138	signed int x_min=0,x_max=0,y_min=0,y_max=0,z_min=0,z_max=0;	138	signed int x_min=0,x_max=0,y_min=0,y_max=0,z_min=0,z_max=0;
139	uint8_t measurement=50,beeper=0;	139	uint8_t measurement=50,beeper=0;
140	unsigned int timer;	140	unsigned int timer;
141		141
142	GRN_ON;	142	GRN_ON;
143	ROT_OFF;	143	ROT_OFF;
144		144
145	while (measurement)	145	while (measurement)
146	{	146	{
147	//H_earth = MM3.x_axisMM3.x_axis + MM3.y_axisMM3.y_axis + MM3.z_axis*MM3.z_axis;	147	//H_earth = MM3.x_axisMM3.x_axis + MM3.y_axisMM3.y_axis + MM3.z_axis*MM3.z_axis;
148		148
149	if (MM3.x_axis > x_max) x_max = MM3.x_axis;	149	if (MM3.x_axis > x_max) x_max = MM3.x_axis;
150	else if (MM3.x_axis < x_min) x_min = MM3.x_axis;	150	else if (MM3.x_axis < x_min) x_min = MM3.x_axis;
151		151
152	if (MM3.y_axis > y_max) y_max = MM3.y_axis;	152	if (MM3.y_axis > y_max) y_max = MM3.y_axis;
153	else if (MM3.y_axis < y_min) y_min = MM3.y_axis;	153	else if (MM3.y_axis < y_min) y_min = MM3.y_axis;
154		154
155	if (MM3.z_axis > z_max) z_max = MM3.z_axis;	155	if (MM3.z_axis > z_max) z_max = MM3.z_axis;
156	else if (MM3.z_axis < z_min) z_min = MM3.z_axis;	156	else if (MM3.z_axis < z_min) z_min = MM3.z_axis;
157		157
158	if (!beeper)	158	if (!beeper)
159	{	159	{
160	ROT_FLASH;	160	ROT_FLASH;
161	GRN_FLASH;	161	GRN_FLASH;
162	beeptime = 50;	162	beeptime = 50;
163	beeper = 50;	163	beeper = 50;
164	}	164	}
165	beeper--;	165	beeper--;
166		166
167	// Schleife mit 100 Hz	167	// Schleife mit 100 Hz
168	timer = SetDelay(10);	168	timer = SetDelay(10);
169	while(!CheckDelay(timer));	169	while(!CheckDelay(timer));
170		170
171	// Wenn Gas zurück genommen wird, Kalibrierung mit 1/2 Sekunde Verzögerung beenden	171	// Wenn Gas zurück genommen wird, Kalibrierung mit 1/2 Sekunde Verzögerung beenden
172	if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--;	172	if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--;
173	}	173	}
174		174
175	// Wertebereich der Achsen	175	// Wertebereich der Achsen
176	MM3_calib.X_range = (x_max - x_min);	176	MM3_calib.X_range = (x_max - x_min);
177	MM3_calib.Y_range = (y_max - y_min);	177	MM3_calib.Y_range = (y_max - y_min);
178	MM3_calib.Z_range = (z_max - z_min);	178	MM3_calib.Z_range = (z_max - z_min);
179		179
180	// Offset der Achsen	180	// Offset der Achsen
181	MM3_calib.X_off = (MM3_calib.X_range / 2) - x_max;	181	MM3_calib.X_off = (MM3_calib.X_range / 2) - x_max;
182	MM3_calib.Y_off = (MM3_calib.Y_range / 2) - y_max;	182	MM3_calib.Y_off = (MM3_calib.Y_range / 2) - y_max;
183	MM3_calib.Z_off = (MM3_calib.Z_range / 2) - z_max;	183	MM3_calib.Z_off = (MM3_calib.Z_range / 2) - z_max;
184		184
185	// und im EEProm abspeichern	185	// und im EEProm abspeichern
186	eeprom_write_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct));	186	eeprom_write_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct));
187	}	187	}
188		188
189		189
190	//############################################################################	190	//############################################################################
191	// Neigungskompensierung und Berechnung der Ausrichtung	191	// Neigungskompensierung und Berechnung der Ausrichtung
192	signed int heading_MM3(void)	192	signed int heading_MM3(void)
193	//############################################################################	193	//############################################################################
194	{	194	{
195	float sin_nick, cos_nick, sin_roll, cos_roll;	195	float sin_nick, cos_nick, sin_roll, cos_roll;
196	float x_corr, y_corr;	196	float x_corr, y_corr;
197	signed int x_axis,y_axis,z_axis, heading;	197	signed int x_axis,y_axis,z_axis, heading;
198	unsigned int div_faktor;	198	unsigned int div_faktor;
199		199
200	div_faktor = (uint16_t)EE_Parameter.UserParam1 * 8;	200	div_faktor = (uint16_t)EE_Parameter.UserParam1 * 8;
201		201
202	// Berechung von sinus und cosinus	202	// Berechung von sinus und cosinus
203	MM3.NickGrad = -(IntegralNick/div_faktor);	203	MM3.NickGrad = -(IntegralNick/div_faktor);
204	sin_nick = sin_f(MM3.NickGrad);	204	sin_nick = sin_f(MM3.NickGrad);
205	cos_nick = cos_f(MM3.NickGrad);	205	cos_nick = cos_f(MM3.NickGrad);
206		206
207	MM3.RollGrad = -(IntegralRoll/div_faktor);	207	MM3.RollGrad = -(IntegralRoll/div_faktor);
208	sin_roll = sin_f(MM3.RollGrad);	208	sin_roll = sin_f(MM3.RollGrad);
209	cos_roll = cos_f(MM3.RollGrad);	209	cos_roll = cos_f(MM3.RollGrad);
210		210
211	// Offset	211	// Offset
212	x_axis = (MM3.x_axis + MM3_calib.X_off);	212	x_axis = (MM3.x_axis + MM3_calib.X_off);
213	y_axis = (MM3.y_axis + MM3_calib.Y_off);	213	y_axis = (MM3.y_axis + MM3_calib.Y_off);
214	z_axis = (MM3.z_axis + MM3_calib.Z_off);	214	z_axis = (MM3.z_axis + MM3_calib.Z_off);
215		215
216	// Normierung Wertebereich	216	// Normierung Wertebereich
217	if ((MM3_calib.X_range > MM3_calib.Y_range) && (MM3_calib.X_range > MM3_calib.Z_range))	217	if ((MM3_calib.X_range > MM3_calib.Y_range) && (MM3_calib.X_range > MM3_calib.Z_range))
218	{	218	{
219	y_axis = ((long)y_axis * MM3_calib.X_range) / MM3_calib.Y_range;	219	y_axis = ((long)y_axis * MM3_calib.X_range) / MM3_calib.Y_range;
220	z_axis = ((long)z_axis * MM3_calib.X_range) / MM3_calib.Z_range;	220	z_axis = ((long)z_axis * MM3_calib.X_range) / MM3_calib.Z_range;
221	}	221	}
222	else if ((MM3_calib.Y_range > MM3_calib.X_range) && (MM3_calib.Y_range > MM3_calib.Z_range))	222	else if ((MM3_calib.Y_range > MM3_calib.X_range) && (MM3_calib.Y_range > MM3_calib.Z_range))
223	{	223	{
224	x_axis = ((long)x_axis * MM3_calib.Y_range) / MM3_calib.X_range;	224	x_axis = ((long)x_axis * MM3_calib.Y_range) / MM3_calib.X_range;
225	z_axis = ((long)z_axis * MM3_calib.Y_range) / MM3_calib.Z_range;	225	z_axis = ((long)z_axis * MM3_calib.Y_range) / MM3_calib.Z_range;
226	}	226	}
227	else //if ((MM3_calib.Z_range > MM3_calib.X_range) && (MM3_calib.Z_range > MM3_calib.Y_range))	227	else //if ((MM3_calib.Z_range > MM3_calib.X_range) && (MM3_calib.Z_range > MM3_calib.Y_range))
228	{	228	{
229	x_axis = ((long)x_axis * MM3_calib.Z_range) / MM3_calib.X_range;	229	x_axis = ((long)x_axis * MM3_calib.Z_range) / MM3_calib.X_range;
230	y_axis = ((long)y_axis * MM3_calib.Z_range) / MM3_calib.Y_range;	230	y_axis = ((long)y_axis * MM3_calib.Z_range) / MM3_calib.Y_range;
231	}	231	}
232		232
233	DebugOut.Analog[9] = x_axis;	233	DebugOut.Analog[9] = x_axis;
234	DebugOut.Analog[10] = y_axis;	234	DebugOut.Analog[10] = y_axis;
235	DebugOut.Analog[11] = z_axis;	235	DebugOut.Analog[11] = z_axis;
236		236
237	// Neigungskompensation	237	// Neigungskompensation
238	x_corr = x_axis * cos_nick;	238	x_corr = x_axis * cos_nick;
239	x_corr += y_axis * sin_roll * sin_nick;	239	x_corr += y_axis * sin_roll * sin_nick;
240	x_corr -= z_axis * cos_roll * sin_nick;	240	x_corr -= z_axis * cos_roll * sin_nick;
241		241
242	y_corr = y_axis * cos_roll;	242	y_corr = y_axis * cos_roll;
243	y_corr += z_axis * sin_roll;	243	y_corr += z_axis * sin_roll;
244		244
245	// Winkelberechnung	245	// Winkelberechnung
246	heading = atan2_i(x_corr, y_corr);	246	heading = atan2_i(x_corr, y_corr);
-		247
-		248	/*
-		249	if (!x_corr && y_corr <0) return (90);
-		250	if (!x_corr && y_corr >0) return (270);
-		251
-		252	heading = atan(y_corr/x_corr)*57.29578;
-		253	if (x_corr < 0) heading = 180-heading;
-		254	if (x_corr > 0 && y_corr < 0) heading = -heading;
-		255	if (x_corr > 0 && y_corr > 0) heading = 360 - heading;
-		256	*/
247		257
248	return (heading);	258	return (heading);
249	}	259	}
250		260

Subversion Repositories FlightCtrl

(root)/branches/v0.60_MicroMag3_Nick666/trunc/compass.c - Rev 378 → 386