Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

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