Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
376 osiair 1
/*
2
 
3
Copyright 2007, Niklas Nold
4
 
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;
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;
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
11
along with this program. If not, see <http://www.gnu.org/licenses/>.
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
14
*/
15
 
16
#include "main.h"
17
 
18
MM3_struct MM3;
19
int8_t Kompass_Offset[2] EEMEM;         // X_off[0], Y_off[1], Z_off[2]
20
int8_t X_off, Y_off, Z_off;
21
 
22
 
23
//############################################################################
24
// Initialisierung
25
void MM3_init(void)
26
//############################################################################
27
{
28
        SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0);        //Interrupt an, Master, 156 kHz Oszillator
29
        //SPSR = (1<<SPI2X);
30
 
31
    DDRB |= (1<<PB7)|(1<<PB5)|(1<<PB2); // J8, MOSI, SCK Ausgang
32
 
33
        PORTD &= ~(1<<PD3);     // J5 auf Low
34
 
35
        MM3.AXIS = MM3_X;
36
        MM3.STATE = MM3_RESET;
37
 
38
        // Kalibrierung aus dem EEprom lesen
39
        X_off = (int8_t)eeprom_read_byte(&Kompass_Offset[0]);
40
        Y_off = (int8_t)eeprom_read_byte(&Kompass_Offset[1]);
41
        Z_off = (int8_t)eeprom_read_byte(&Kompass_Offset[2]);
42
 
43
 
44
        //X_off = -11;
45
        //Y_off = 30;
46
        //Z_off = 18;
47
 
48
}
49
 
50
 
51
//############################################################################
52
// Wird in der SIGNAL (SIG_OVERFLOW0) aufgerufen
53
void MM3_timer0(void)
54
//############################################################################
55
{
56
        switch (MM3.STATE)
57
        {
58
        case MM3_RESET:                        
59
                PORTB |= (1<<PB2);      // J8 auf High, MM3 Reset
60
                MM3.STATE = MM3_START_TRANSFER;
61
                return;
62
 
63
        case MM3_START_TRANSFER:
64
                PORTB &= ~(1<<PB2);     // J8 auf Low (war ~125 µs auf High)            
65
 
66
                if (MM3.AXIS == MM3_X) SPDR = 0x31;                     // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus
67
                else if (MM3.AXIS == MM3_Y) SPDR = 0x32;                // Micromag Period Select ist auf 256 (0x30)
68
                else if (MM3.AXIS == MM3_Z) SPDR = 0x33;                // 1: x-Achse, 2: Y-Achse, 3: Z-Achse
69
 
70
                MM3.DRDY = SetDelay(8);         // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms)
71
                MM3.STATE = MM3_WAIT_DRDY;
72
                return;
73
 
74
        case MM3_WAIT_DRDY:
75
                if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei
76
                return;                                                 // Jetzt gehts weiter in SIGNAL (SIG_SPI)
77
        /*
78
        case MM3_TILT:                                          // Zeitnahe Speicherung der aktuellen Neigung in °
79
                MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8);
80
                MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8);
81
 
82
                MM3.AXIS = MM3_X;
83
                MM3.STATE = MM3_RESET;
84
                return;
85
        */
86
        }
87
}
88
 
89
 
90
//############################################################################
91
// SPI byte ready
92
SIGNAL (SIG_SPI)
93
//############################################################################
94
{      
95
        switch (MM3.STATE)
96
        {      
97
        case MM3_DRDY:          // 1. Byte ist da, abspeichern, an die MSB-Stelle rücken                
98
                if (MM3.AXIS == MM3_X)
99
                {
100
                        MM3.x_axis = SPDR;
101
                        MM3.x_axis <<= 8;
102
                }
103
                else if (MM3.AXIS == MM3_Y)
104
                {
105
                        MM3.y_axis = SPDR;
106
                        MM3.y_axis <<= 8;
107
                }
108
                else    // if (MM3.AXIS == MM3_Z)
109
                {
110
                        MM3.z_axis = SPDR;
111
                        MM3.z_axis <<= 8;
112
                }
113
 
114
                SPDR=0x00;              // Übertragung von 2. Byte auslösen
115
                MM3.STATE=MM3_BYTE2;
116
                return;
117
 
118
        case MM3_BYTE2:         // 2. Byte der entsprechenden Achse ist da              
119
                if (MM3.AXIS == MM3_X)
120
                {
121
                        MM3.x_axis |= SPDR;
122
                        // Spikes filtern
123
                        if (abs(MM3.x_axis) < Max_Axis_Value) MM3.x_axis_old = MM3.x_axis;
124
                        else MM3.x_axis = MM3.x_axis_old;
125
                        MM3.AXIS = MM3_Y;
126
                        MM3.STATE = MM3_RESET;
127
                }
128
                else if (MM3.AXIS == MM3_Y)
129
                {
130
                        MM3.y_axis |= SPDR;
131
                        if (abs(MM3.y_axis) < Max_Axis_Value) MM3.y_axis_old = MM3.y_axis;
132
                        else MM3.y_axis = MM3.y_axis_old;              
133
                        MM3.AXIS = MM3_Z;
134
                        MM3.STATE = MM3_RESET;
135
                }
136
                else    // if (MM3.AXIS == MM3_Z) 
137
                {
138
                        MM3.z_axis |= SPDR;
139
                        if (abs(MM3.z_axis) < Max_Axis_Value) MM3.z_axis_old = MM3.z_axis;
140
                        else MM3.z_axis = MM3.z_axis_old;
141
                        MM3.AXIS = MM3_X;
142
                        MM3.STATE = MM3_RESET;
143
                }
144
 
145
                return;
146
        }
147
}
148
 
149
//############################################################################
150
// Kompass kalibrieren
151
void MM3_calib(void)
152
//############################################################################
153
{
154
        signed int x_min=0,x_max=0,y_min=0,y_max=0,z_min=0,z_max=0;
155
        uint8_t measurement=50,beeper=0;
156
        unsigned int timer;
157
 
158
        while (measurement)
159
        {
160
                //H_earth = MM3.x_axis*MM3.x_axis + MM3.y_axis*MM3.y_axis + MM3.z_axis*MM3.z_axis;
161
 
162
                if (MM3.x_axis > x_max) x_max = MM3.x_axis;
163
                else if (MM3.x_axis < x_min) x_min = MM3.x_axis;
164
 
165
                if (MM3.y_axis > y_max) y_max = MM3.y_axis;
166
                else if (MM3.y_axis < y_min) y_min = MM3.y_axis;
167
 
168
                if (MM3.z_axis > z_max) z_max = MM3.z_axis;
169
                else if (MM3.z_axis < z_min) z_min = MM3.z_axis;
170
 
171
                if (!beeper)
172
                {
173
                        beeper = 50;
174
                        beeptime = 50;
175
                }
176
                beeper--;
177
 
178
                // Schleife mit 100 Hz voll ausreichend
179
                timer = SetDelay(10);
180
                while(!CheckDelay(timer));
181
 
182
                // Wenn Gas zurück genommen wird, Kalibrierung mit Verzögerung beenden
183
                if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--;
184
        }
185
 
186
        // Offset der Achsen berechnen
187
        X_off = (x_max + x_min) / 2;
188
        Y_off = (y_max + y_min) / 2;
189
        Z_off = (z_max + z_min) / 2;
190
 
191
        // und im EEProm abspeichern
192
        eeprom_write_byte(&Kompass_Offset[0], X_off);
193
        eeprom_write_byte(&Kompass_Offset[1], Y_off);
194
        eeprom_write_byte(&Kompass_Offset[2], Z_off);
195
 
196
}
197
 
198
 
199
//############################################################################
200
// Neigungskompensierung und Berechnung der Ausrichtung
201
signed int MM3_heading(void)
202
//############################################################################
203
{
204
        float sin_nick, cos_nick, sin_roll, cos_roll;
205
        signed int x_corr, y_corr, heading;
206
        signed int x_axis,y_axis,z_axis;
207
 
208
        MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam4*8);
209
        MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam4*8);
210
 
211
        // Berechung von sinus und cosinus
212
        sin_nick = sin_f(MM3.NickGrad);
213
        cos_nick = cos_f(MM3.NickGrad);
214
        sin_roll = sin_f(MM3.RollGrad);
215
        cos_roll = cos_f(MM3.RollGrad);
216
 
217
        // Offset der Achsen nur bei Bedarf (also hier) berücksichtigen
218
        x_axis = MM3.x_axis - X_off;
219
        y_axis = MM3.y_axis - Y_off;
220
        z_axis = MM3.z_axis - Z_off;   
221
 
222
    // Neigungskompensation
223
        x_corr = (cos_nick * x_axis) - (((sin_roll *  y_axis) - (cos_roll * z_axis)) * sin_nick);
224
        y_corr = ((cos_roll * y_axis) + (sin_roll * z_axis));
225
 
226
        // Winkelberechnung
227
        heading = atan2_i(x_corr, y_corr);
228
 
229
return (heading);
230
}