Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
700 killagreg 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
#include <stdlib.h>
16
#include <avr/io.h>
17
#include <avr/interrupt.h>
18
 
19
#include "mm3.h"
20
#include "main.h"
21
#include "mymath.h"
22
#include "fc.h"
23
#include "timer0.h"
24
#include "rc.h"
25
#include "eeprom.h"
26
 
27
#define MAX_AXIS_VALUE          500
28
 
29
 
30
typedef struct
31
{
32
        uint8_t STATE;
33
        uint16_t DRDY;
34
        uint8_t AXIS;
35
        int16_t x_axis;
36
        int16_t y_axis;
37
        int16_t z_axis;
38
} MM3_working_t;
39
 
40
 
41
// MM3 State Machine
42
#define MM3_STATE_RESET                         0
43
#define MM3_STATE_START_TRANSFER        1
44
#define MM3_STATE_WAIT_DRDY                     2
45
#define MM3_STATE_DRDY                          3
46
#define MM3_STATE_BYTE2                         4
47
 
48
#define MM3_X_AXIS              0x01
49
#define MM3_Y_AXIS              0x02
50
#define MM3_Z_AXIS              0x03
51
 
52
 
53
#define MM3_PERIOD_32   0x00
54
#define MM3_PERIOD_64   0x10
55
#define MM3_PERIOD_128  0x20
56
#define MM3_PERIOD_256  0x30
57
#define MM3_PERIOD_512  0x40
58
#define MM3_PERIOD_1024 0x50
59
#define MM3_PERIOD_2048 0x60
60
#define MM3_PERIOD_4096 0x70
61
 
62
MM3_calib_t MM3_calib;
63
volatile MM3_working_t MM3;
64
 
65
 
66
 
67
/*********************************************/
68
/*  Initialize Interface to MM3 Compass      */
69
/*********************************************/
726 killagreg 70
void MM3_Init(void)
700 killagreg 71
{
72
        uint8_t sreg = SREG;
73
 
74
        cli();
75
 
76
        // Configure Pins for SPI
77
        // set SCK (PB7), MOSI (PB5) as output
78
        DDRB |= (1<<DDB7)|(1<<DDB5);
79
        // set MISO (PB6) as input
80
        DDRB &= ~(1<<DDB6);
81
 
82
        // Output Pins PC4->MM3_SS ,PC5->MM3_RESET
83
        DDRC |= (1<<DDC4)|(1<<DDC5);
84
        // set pins permanent to low
85
        PORTC &= ~((1<<PORTC4)|(1<<PORTC5));
86
 
87
        // Initialize SPI-Interface
88
        // Enable interrupt (SPIE=1)
89
        // Enable SPI bus (SPE=1)
90
        // MSB transmitted first (DORD = 0)
91
        // Master SPI Mode (MSTR=1)
92
        // Clock polarity low whn idle (CPOL=0)
93
        // clock phase sample at leading edge (CPHA=0)
94
        // clock rate = SYSCLK/128 (SPI2X=0, SPR1=1, SPR0=1) 20MHz/128 = 156.25kHz
95
        SPCR = (1<<SPIE)|(1<<SPE)|(0<<DORD)|(1<<MSTR)|(0<<CPOL)|(0<<CPHA)|(1<<SPR1)|(1<<SPR0);
96
        SPSR &= ~(1<<SPI2X);
97
 
98
    // Init Statemachine
99
        MM3.AXIS = MM3_X_AXIS;
100
        MM3.STATE = MM3_STATE_RESET;
101
 
102
        // Read calibration from EEprom
103
        MM3_calib.X_off = (int8_t)GetParamByte(PID_MM3_X_OFF);
104
        MM3_calib.Y_off = (int8_t)GetParamByte(PID_MM3_Y_OFF);
105
        MM3_calib.Z_off = (int8_t)GetParamByte(PID_MM3_Z_OFF);
106
        MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE);
107
        MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE);
108
        MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE);
109
 
110
        SREG = sreg;
111
}
112
 
113
 
114
/*********************************************/
115
/*  Get Data from MM3                        */
116
/*********************************************/
726 killagreg 117
void MM3_Update() // called every 102.4 ms by timer 0 ISR
700 killagreg 118
{
119
        switch (MM3.STATE)
120
        {
121
        case MM3_STATE_RESET:
727 killagreg 122
                PORTC &= ~(1<<PORTC4); // select slave
700 killagreg 123
                PORTC |= (1<<PORTC5);   // PC5 to High, MM3 Reset
124
                MM3.STATE = MM3_STATE_START_TRANSFER;
125
                return;
126
 
127
        case MM3_STATE_START_TRANSFER:
128
                PORTC &= ~(1<<PORTC5);  // PC4 auf Low (was 102.4 µs at high level)
129
 
130
                // write to SPDR triggers automatically the transfer MOSI MISO
131
                // MM3 Period, + AXIS code
132
                if (MM3.AXIS == MM3_X_AXIS) SPDR = MM3_PERIOD_256 + MM3_X_AXIS;
133
                else if (MM3.AXIS == MM3_Y_AXIS) SPDR = MM3_PERIOD_256 + MM3_Y_AXIS;
134
                else SPDR = MM3_PERIOD_256 + MM3_Z_AXIS; // MM3_Z_AXIS
135
 
136
                // DRDY line is not connected, therefore
137
                // wait before reading data back
138
                MM3.DRDY = SetDelay(8); // wait 8ms for data ready
139
                MM3.STATE = MM3_STATE_WAIT_DRDY;
140
                return;
141
 
142
        case MM3_STATE_WAIT_DRDY:
143
                if (CheckDelay(MM3.DRDY))
144
                {
145
                        // write something into SPDR to trigger data reading
146
                        SPDR = 0x00;
147
                        MM3.STATE = MM3_STATE_DRDY;
148
                }
149
                return;
150
        }
151
}
152
 
153
 
154
/*********************************************/
155
/*  Interrupt SPI transfer complete          */
156
/*********************************************/
157
ISR(SPI_STC_vect)
158
{
159
        static int8_t tmp;
160
        int16_t value;
161
 
162
        switch (MM3.STATE)
163
        {
164
        // 1st byte received
165
        case MM3_STATE_DRDY:
166
                tmp = SPDR;     // store 1st byte
167
                SPDR = 0x00;    // trigger transfer of 2nd byte
168
                MM3.STATE = MM3_STATE_BYTE2;
169
                return;
170
 
171
        case MM3_STATE_BYTE2:           // 2nd byte received
172
                value = (int16_t)tmp;   // combine the 1st and 2nd byte to a word
173
                value <<= 8;            // shift 1st byte to MSB-Position
174
                value |= (int16_t)SPDR; // add 2nd byte
175
 
176
                if(abs(value) < MAX_AXIS_VALUE)         // ignore spikes
177
                {
178
                        switch (MM3.AXIS)
179
                        {
180
                        case MM3_X_AXIS:
181
                                MM3.x_axis = value;
182
                                MM3.AXIS = MM3_Y_AXIS;
183
                                break;
184
                        case MM3_Y_AXIS:
185
                                MM3.y_axis = value;
186
                                MM3.AXIS = MM3_Z_AXIS;
187
                                break;
188
                        case MM3_Z_AXIS:
189
                                MM3.z_axis = value;
190
                                MM3.AXIS = MM3_X_AXIS;
191
                                break;
192
                        default:
193
                                MM3.AXIS = MM3_X_AXIS;
194
                                break;
195
                        }
196
                }
727 killagreg 197
                PORTC |= (1<<PORTC4); // deselect slave
700 killagreg 198
                MM3.STATE = MM3_STATE_RESET;
199
        }
200
}
201
 
202
 
203
 
204
/*********************************************/
205
/*  Calibrate Compass                        */
206
/*********************************************/
726 killagreg 207
void MM3_Calibrate(void)
700 killagreg 208
{
209
        int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0;
210
        uint8_t measurement = 50, beeper = 0;
211
        uint16_t timer;
212
 
213
        GRN_ON;
214
        ROT_OFF;
215
 
216
        // get maximum and minimum reading of all axis
217
        while (measurement)
218
        {
219
                if (MM3.x_axis > x_max) x_max = MM3.x_axis;
220
                else if (MM3.x_axis < x_min) x_min = MM3.x_axis;
221
 
222
                if (MM3.y_axis > y_max) y_max = MM3.y_axis;
223
                else if (MM3.y_axis < y_min) y_min = MM3.y_axis;
224
 
225
                if (MM3.z_axis > z_max) z_max = MM3.z_axis;
226
                else if (MM3.z_axis < z_min) z_min = MM3.z_axis;
227
 
228
                if (!beeper)
229
                {
230
                        ROT_FLASH;
231
                        GRN_FLASH;
232
                        BeepTime = 50;
233
                        beeper = 50;
234
                }
235
                beeper--;
236
 
237
                // loop with period of 10 ms / 100 Hz
238
                timer = SetDelay(10);
239
                while(!CheckDelay(timer));
240
 
707 killagreg 241
                // If thrust is less than 100, stop calibration with a delay of 0.5 seconds
242
                if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--;
700 killagreg 243
        }
244
 
245
        // Rage of all axis
246
        MM3_calib.X_range = (x_max - x_min);
247
        MM3_calib.Y_range = (y_max - y_min);
248
        MM3_calib.Z_range = (z_max - z_min);
249
 
250
        // Offset of all axis
251
        MM3_calib.X_off = (x_max + x_min) / 2;
252
        MM3_calib.Y_off = (y_max + y_min) / 2;
253
        MM3_calib.Z_off = (z_max + z_min) / 2;
254
 
255
        // save to EEProm
256
        SetParamByte(PID_MM3_X_OFF,   (uint8_t)MM3_calib.X_off);
257
        SetParamByte(PID_MM3_Y_OFF,   (uint8_t)MM3_calib.Y_off);
258
        SetParamByte(PID_MM3_Z_OFF,   (uint8_t)MM3_calib.Z_off);
259
        SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range);
260
        SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range);
261
        SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range);
262
}
263
 
264
 
265
/*********************************************/
266
/*  Calculate north direction (heading)      */
267
/*********************************************/
726 killagreg 268
int16_t MM3_Heading(void)
700 killagreg 269
{
701 killagreg 270
        int32_t sin_pitch, cos_pitch, sin_roll, cos_roll, sin_yaw, cos_yaw;
700 killagreg 271
        int32_t  Hx, Hy, Hz, Hx_corr, Hy_corr;
272
        int16_t angle;
273
        uint16_t div_factor;
274
        int16_t heading;
275
 
276
        // calibration factor for transforming Gyro Integrals to angular degrees
277
        div_factor = (uint16_t)ParamSet.UserParam3 * 8;
278
 
279
        // Offset correction and normalization (values of H are +/- 512)
280
        Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range;
281
        Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range;
282
        Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range;
283
 
284
        // Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation
285
    // assuming the MM3 board is mounted parallel to the frame.
286
    // User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading
287
    // in a top view counter clockwise direction.
288
    // North is in opposite direction of the small arrow on the MM3 board.
289
    // Therefore 180 deg must be added to that angle.
290
        angle = ((int16_t)ParamSet.UserParam4 + 180);
291
        // wrap angle to interval of 0°- 359°
292
        angle += 360;
293
        angle %= 360;
294
        sin_yaw = (int32_t)(c_sin_8192(angle));
295
        cos_yaw = (int32_t)(c_cos_8192(angle));
296
 
297
        Hx_corr = Hx;
298
        Hy_corr = Hy;
299
 
300
        // rotate
301
        Hx = (Hx_corr * cos_yaw - Hy_corr  * sin_yaw) / 8192;
302
        Hy = (Hx_corr * sin_yaw + Hy_corr  * cos_yaw) / 8192;
303
 
304
 
305
    // tilt compensation
306
 
307
        // calibration factor for transforming Gyro Integrals to angular degrees
308
        div_factor = (uint16_t)ParamSet.UserParam3 * 8;
309
 
701 killagreg 310
        // calculate sinus cosinus of pitch and tilt angle
311
        angle = (IntegralPitch/div_factor);
312
        sin_pitch = (int32_t)(c_sin_8192(angle));
313
        cos_pitch = (int32_t)(c_cos_8192(angle));
700 killagreg 314
 
315
        angle = (IntegralRoll/div_factor);
316
        sin_roll = (int32_t)(c_sin_8192(angle));
317
        cos_roll = (int32_t)(c_cos_8192(angle));
318
 
701 killagreg 319
        Hx_corr = Hx * cos_pitch;
320
        Hx_corr -= Hz * sin_pitch;
700 killagreg 321
        Hx_corr /= 8192;
322
 
323
        Hy_corr = Hy * cos_roll;
324
        Hy_corr += Hz * sin_roll;
325
        Hy_corr /= 8192;
326
 
327
        // calculate Heading
328
        heading = c_atan2(Hy_corr, Hx_corr);
329
 
727 killagreg 330
    // atan returns angular range from -180 deg to 180 deg in counter clockwise notation
331
    // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
332
        if (heading < 0) heading = -heading;
333
        else heading = 360 - heading;
700 killagreg 334
 
335
return heading;
336
}