Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1612 dongfang 1
/*
2
 
3
Copyright 2008, by Killagreg
4
 
5
This program (files mm3.c and mm3.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: The original implementation was done by Niklas Nold.
14
All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de
15
*/
16
#include <stdlib.h>
17
#include <avr/io.h>
18
#include <avr/interrupt.h>
19
#include <inttypes.h>
20
 
21
#include "mm3.h"
22
#include "mymath.h"
23
#include "timer0.h"
24
#include "rc.h"
25
#include "eeprom.h"
26
#include "printf_P.h"
27
 
28
 
29
// for compatibility reasons gcc3.x <-> gcc4.x
30
#ifndef SPCR
31
#define SPCR   SPCR0
32
#endif
33
#ifndef SPIE
34
#define SPIE   SPIE0
35
#endif
36
#ifndef SPE
37
#define SPE    SPE0
38
#endif
39
#ifndef DORD
40
#define DORD   DORD0
41
#endif
42
#ifndef MSTR
43
#define MSTR   MSTR0
44
#endif
45
#ifndef CPOL
46
#define CPOL   CPOL0
47
#endif
48
#ifndef CPHA
49
#define CPHA   CPHA0
50
#endif
51
#ifndef SPR1
52
#define SPR1   SPR01
53
#endif
54
#ifndef SPR0
55
#define SPR0   SPR00
56
#endif
57
 
58
#ifndef SPDR
59
#define SPDR   SPDR0
60
#endif
61
 
62
#ifndef SPSR
63
#define SPSR   SPSR0
64
#endif
65
#ifndef SPIF
66
#define SPIF   SPIF0
67
#endif
68
#ifndef WCOL
69
#define WCOL   WCOL0
70
#endif
71
#ifndef SPI2X
72
#define SPI2X  SPI2X0
73
#endif
74
// -------------------------
75
 
76
 
77
#define MAX_AXIS_VALUE          500
78
 
79
 
80
typedef struct
81
{
82
        uint8_t STATE;
83
        uint16_t DRDY;
84
        uint8_t AXIS;
85
        int16_t x_axis;
86
        int16_t y_axis;
87
        int16_t z_axis;
88
} MM3_working_t;
89
 
90
 
91
// MM3 State Machine
92
#define MM3_STATE_RESET                         0
93
#define MM3_STATE_START_TRANSFER        1
94
#define MM3_STATE_WAIT_DRDY                     2
95
#define MM3_STATE_DRDY                          3
96
#define MM3_STATE_BYTE2                         4
97
 
98
#define MM3_X_AXIS              0x01
99
#define MM3_Y_AXIS              0x02
100
#define MM3_Z_AXIS              0x03
101
 
102
 
103
#define MM3_PERIOD_32   0x00
104
#define MM3_PERIOD_64   0x10
105
#define MM3_PERIOD_128  0x20
106
#define MM3_PERIOD_256  0x30
107
#define MM3_PERIOD_512  0x40
108
#define MM3_PERIOD_1024 0x50
109
#define MM3_PERIOD_2048 0x60
110
#define MM3_PERIOD_4096 0x70
111
 
112
#if defined(USE_WALTER_EXT) // walthers board
113
        // Output Pins (J9)PC6->MM3_SS ,(J8)PB2->MM3_RESET
114
        #define MM3_SS_PORT    PORTC //J9->MM3_SS
115
        #define MM3_SS_DDR     DDRC
116
        #define MM3_SS_PIN     PC6
117
        #define MM3_RESET_PORT PORTB //J8->MM3_RESET
118
        #define MM3_RESET_DDR  DDRB
119
        #define MM3_RESET_PIN  PB2
120
#elif defined(USE_NICK666) // nick666 version 0.67g
121
        #define MM3_SS_PORT    PORTD //J5->MM3_SS
122
        #define MM3_SS_DDR     DDRD
123
        #define MM3_SS_PIN     PD3
124
        #define MM3_RESET_PORT PORTB //J8->MM3_RESET
125
        #define MM3_RESET_DDR  DDRB
126
        #define MM3_RESET_PIN  PB2
127
#else // killagregs board
128
        // Output Pins PC4->MM3_SS ,PC5->MM3_RESET
129
        #define MM3_SS_PORT    PORTC
130
        #define MM3_SS_DDR     DDRC
131
        #define MM3_SS_PIN     PC4
132
        #define MM3_RESET_PORT PORTC
133
        #define MM3_RESET_DDR  DDRC
134
        #define MM3_RESET_PIN  PC5
135
#endif
136
 
137
#define MM3_SS_ON      MM3_SS_PORT    &= ~(1<<MM3_SS_PIN);
138
#define MM3_SS_OFF     MM3_SS_PORT    |=  (1<<MM3_SS_PIN);
139
#define MM3_RESET_ON   MM3_RESET_PORT |=  (1<<MM3_RESET_PIN);
140
#define MM3_RESET_OFF  MM3_RESET_PORT  &= ~(1<<MM3_RESET_PIN);
141
 
142
 
143
 
144
MM3_calib_t MM3_calib;
145
volatile MM3_working_t MM3;
146
volatile uint8_t MM3_Timeout = 0;
147
 
148
 
149
 
150
/*********************************************/
151
/*  Initialize Interface to MM3 Compass      */
152
/*********************************************/
153
void MM3_Init(void)
154
{
155
        uint8_t sreg = SREG;
156
 
157
        cli();
158
 
159
        // Configure Pins for SPI
160
        // set SCK (PB7), MOSI (PB5) as output
161
        DDRB |= (1<<DDB7)|(1<<DDB5);
162
        // set MISO (PB6) as input
163
        DDRB &= ~(1<<DDB6);
164
 
165
 
166
        // Output Pins MM3_SS ,MM3_RESET
167
        MM3_SS_DDR    |= (1<<MM3_SS_PIN);
168
        MM3_RESET_DDR |= (1<<MM3_RESET_PIN);
169
        // set pins permanent to low
170
        MM3_SS_PORT    &= ~((1<<MM3_SS_PIN));
171
        MM3_RESET_PORT &= ~((1<<MM3_RESET_PIN));
172
 
173
        // Initialize SPI-Interface
174
        // Enable interrupt (SPIE=1)
175
        // Enable SPI bus (SPE=1)
176
        // MSB transmitted first (DORD = 0)
177
        // Master SPI Mode (MSTR=1)
178
        // Clock polarity low when idle (CPOL=0)
179
        // Clock phase sample at leading edge (CPHA=0)
180
        // Clock rate = SYSCLK/128 (SPI2X=0, SPR1=1, SPR0=1) 20MHz/128 = 156.25kHz
181
        SPCR = (1<<SPIE)|(1<<SPE)|(0<<DORD)|(1<<MSTR)|(0<<CPOL)|(0<<CPHA)|(1<<SPR1)|(1<<SPR0);
182
        SPSR &= ~(1<<SPI2X);
183
 
184
    // Init Statemachine
185
        MM3.AXIS = MM3_X_AXIS;
186
        MM3.STATE = MM3_STATE_RESET;
187
 
188
        // Read calibration from EEprom
189
        MM3_calib.X_off = (int8_t)GetParamByte(PID_MM3_X_OFF);
190
        MM3_calib.Y_off = (int8_t)GetParamByte(PID_MM3_Y_OFF);
191
        MM3_calib.Z_off = (int8_t)GetParamByte(PID_MM3_Z_OFF);
192
        MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE);
193
        MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE);
194
        MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE);
195
 
196
        MM3_Timeout = 0;
197
 
198
        SREG = sreg;
199
}
200
 
201
 
202
/*********************************************/
203
/*  Get Data from MM3                        */
204
/*********************************************/
205
void MM3_Update(void) // called every 102.4 µs by timer 0 ISR
206
{
207
        switch (MM3.STATE)
208
        {
209
        case MM3_STATE_RESET:
210
                MM3_SS_ON  // select slave
211
                MM3_RESET_ON    // RESET to High, MM3 Reset
212
                MM3.STATE = MM3_STATE_START_TRANSFER;
213
                return;
214
 
215
        case MM3_STATE_START_TRANSFER:
216
                MM3_RESET_OFF   // RESET auf Low (was 102.4 µs at high level)
217
                // write to SPDR triggers automatically the transfer MOSI MISO
218
                // MM3 Period, + AXIS code
219
                switch(MM3.AXIS)
220
                {
221
                case MM3_X_AXIS:
222
                        SPDR = MM3_PERIOD_256 + MM3_X_AXIS;
223
                        break;
224
                case MM3_Y_AXIS:
225
                        SPDR = MM3_PERIOD_256 + MM3_Y_AXIS;
226
                        break;
227
                case MM3_Z_AXIS:
228
                        SPDR = MM3_PERIOD_256 + MM3_Z_AXIS;
229
                        break;
230
                default:
231
                        MM3.AXIS = MM3_X_AXIS;
232
                        MM3.STATE = MM3_STATE_RESET;
233
                        return;
234
                }
235
 
236
                // DRDY line is not connected, therefore
237
                // wait before reading data back
238
                MM3.DRDY = SetDelay(8); // wait 8ms for data ready
239
                MM3.STATE = MM3_STATE_WAIT_DRDY;
240
                return;
241
 
242
        case MM3_STATE_WAIT_DRDY:
243
                if (CheckDelay(MM3.DRDY))
244
                {
245
                        // write something into SPDR to trigger data reading
246
                        SPDR = 0x00;
247
                        MM3.STATE = MM3_STATE_DRDY;
248
                }
249
                return;
250
        }
251
}
252
 
253
 
254
/*********************************************/
255
/*  Interrupt SPI transfer complete          */
256
/*********************************************/
257
ISR(SPI_STC_vect)
258
{
259
        static int8_t tmp;
260
        int16_t value;
261
 
262
        switch (MM3.STATE)
263
        {
264
        // 1st byte received
265
        case MM3_STATE_DRDY:
266
                tmp = SPDR;     // store 1st byte
267
                SPDR = 0x00;    // trigger transfer of 2nd byte
268
                MM3.STATE = MM3_STATE_BYTE2;
269
                return;
270
 
271
        case MM3_STATE_BYTE2:           // 2nd byte received
272
                value = (int16_t)tmp;   // combine the 1st and 2nd byte to a word
273
                value <<= 8;            // shift 1st byte to MSB-Position
274
                value |= (int16_t)SPDR; // add 2nd byte
275
 
276
                if(abs(value) < MAX_AXIS_VALUE)         // ignore spikes
277
                {
278
                        switch (MM3.AXIS)
279
                        {
280
                        case MM3_X_AXIS:
281
                                MM3.x_axis = value;
282
                                MM3.AXIS = MM3_Y_AXIS;
283
                                break;
284
                        case MM3_Y_AXIS:
285
                                MM3.y_axis = value;
286
                                MM3.AXIS = MM3_Z_AXIS;
287
                                break;
288
                        case MM3_Z_AXIS:
289
                                MM3.z_axis = value;
290
                                MM3.AXIS = MM3_X_AXIS;
291
                                break;
292
                        default:
293
                                MM3.AXIS = MM3_X_AXIS;
294
                                break;
295
                        }
296
                }
297
                MM3_SS_OFF // deselect slave
298
                MM3.STATE = MM3_STATE_RESET;
299
                // Update timeout is called every 102.4 µs.
300
                // It takes 2 cycles to write a measurement data request for one axis and
301
                // at at least 8 ms / 102.4 µs = 79 cycles to read the requested data back.
302
                // I.e. 81 cycles * 102.4 µs = 8.3ms per axis.
303
                // The two function accessing the MM3 Data - MM3_Calibrate() and MM3_Heading() -
304
                // decremtent the MM3_Timeout every 100 ms.
305
                // incrementing the counter by 1 every 8.3 ms is sufficient to avoid a timeout.
306
                if ((MM3.x_axis != MM3.y_axis) || (MM3.x_axis != MM3.z_axis) || (MM3.y_axis != MM3.z_axis))
307
                {       // if all axis measurements give diffrent readings the data should be valid
308
                        if(MM3_Timeout < 20) MM3_Timeout++;
309
                }
310
                else // something is very strange here
311
                {
312
                        if(MM3_Timeout ) MM3_Timeout--;
313
                }
314
                return;
315
 
316
        default:
317
                return;
318
        }
319
}
320
 
321
 
322
/*********************************************/
323
/*  Calibrate Compass                        */
324
/*********************************************/
325
void MM3_Calibrate(void)
326
{
327
        static int16_t x_min, x_max, y_min, y_max, z_min, z_max;
328
 
329
        switch(CompassCalState)
330
        {
331
                case 1: // change to x-y axis
332
                        x_min =  10000;
333
                        x_max = -10000;
334
                        y_min =  10000;
335
                        y_max = -10000;
336
                        z_min =  10000;
337
                        z_max = -10000;
338
                        break;
339
                case 2:
340
                        // find Min and Max of the X- and Y-Axis
341
                        if(MM3.x_axis < x_min) x_min = MM3.x_axis;
342
                        if(MM3.x_axis > x_max) x_max = MM3.x_axis;
343
                        if(MM3.y_axis < y_min) y_min = MM3.y_axis;
344
                        if(MM3.y_axis > y_max) y_max = MM3.y_axis;
345
                        break;
346
                case 3:
347
                        // change to z-Axis
348
                break;
349
                case 4:
350
                        RED_ON;  // find Min and Max of the Z-axis
351
                        if(MM3.z_axis < z_min) z_min = MM3.z_axis;
352
                        if(MM3.z_axis > z_max) z_max = MM3.z_axis;
353
                break;
354
                case 5:
355
                        // calc range of all axis
356
                        MM3_calib.X_range = (x_max - x_min);
357
                        MM3_calib.Y_range = (y_max - y_min);
358
                        MM3_calib.Z_range = (z_max - z_min);
359
 
360
                        // calc offset of all axis
361
                        MM3_calib.X_off = (x_max + x_min) / 2;
362
                        MM3_calib.Y_off = (y_max + y_min) / 2;
363
                        MM3_calib.Z_off = (z_max + z_min) / 2;
364
 
365
                        // save to EEProm
366
                        SetParamByte(PID_MM3_X_OFF,   (uint8_t)MM3_calib.X_off);
367
                        SetParamByte(PID_MM3_Y_OFF,   (uint8_t)MM3_calib.Y_off);
368
                        SetParamByte(PID_MM3_Z_OFF,   (uint8_t)MM3_calib.Z_off);
369
                        SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range);
370
                        SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range);
371
                        SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range);
372
 
373
                        CompassCalState = 0;
374
                        break;
375
                default:
376
                        CompassCalState = 0;
377
                        break;
378
        }
379
}
380
 
381
 
382
/*
383
void MM3_Calibrate(void)
384
{
385
        static uint8_t debugcounter = 0;
386
        int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0;
387
        uint8_t measurement = 50, beeper = 0;
388
        uint16_t timer;
389
 
390
        GRN_ON;
391
        RED_OFF;
392
 
393
        // get maximum and minimum reading of all axis
394
        while (measurement)
395
        {
396
                // reset range markers if yawstick ist leftmost
397
                if(PPM_in[staticParams.ChannelAssignment[CH_YAW]] > 100)
398
                {
399
                        x_min = 0;
400
                        x_max = 0;
401
                        y_min = 0;
402
                        y_max = 0;
403
                        z_min = 0;
404
                        z_max = 0;
405
                }
406
 
407
                if (MM3.x_axis > x_max) x_max = MM3.x_axis;
408
                else if (MM3.x_axis < x_min) x_min = MM3.x_axis;
409
 
410
                if (MM3.y_axis > y_max) y_max = MM3.y_axis;
411
                else if (MM3.y_axis < y_min) y_min = MM3.y_axis;
412
 
413
                if (MM3.z_axis > z_max) z_max = MM3.z_axis;
414
                else if (MM3.z_axis < z_min) z_min = MM3.z_axis;
415
 
416
                if (!beeper)
417
                {
418
                        RED_FLASH;
419
                        GRN_FLASH;
420
                        BeepTime = 50;
421
                        beeper = 50;
422
                }
423
                beeper--;
424
                // loop with period of 10 ms / 100 Hz
425
                timer = SetDelay(10);
426
                while(!CheckDelay(timer));
427
 
428
                if(debugcounter++ > 30)
429
                {
430
                        printf("\n\rXMin:%4d, XMax:%4d, YMin:%4d, YMax:%4d, ZMin:%4d, ZMax:%4d",x_min,x_max,y_min,y_max,z_min,z_max);
431
                        debugcounter = 0;
432
                }
433
 
434
                // If gas is less than 100, stop calibration with a delay of 0.5 seconds
435
                if (PPM_in[staticParams.ChannelAssignment[CH_GAS]] < 100) measurement--;
436
        }
437
        // Rage of all axis
438
        MM3_calib.X_range = (x_max - x_min);
439
        MM3_calib.Y_range = (y_max - y_min);
440
        MM3_calib.Z_range = (z_max - z_min);
441
 
442
        // Offset of all axis
443
        MM3_calib.X_off = (x_max + x_min) / 2;
444
        MM3_calib.Y_off = (y_max + y_min) / 2;
445
        MM3_calib.Z_off = (z_max + z_min) / 2;
446
 
447
        // save to EEProm
448
        SetParamByte(PID_MM3_X_OFF,   (uint8_t)MM3_calib.X_off);
449
        SetParamByte(PID_MM3_Y_OFF,   (uint8_t)MM3_calib.Y_off);
450
        SetParamByte(PID_MM3_Z_OFF,   (uint8_t)MM3_calib.Z_off);
451
        SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range);
452
        SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range);
453
        SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range);
454
 
455
}
456
*/
457
 
458
/*********************************************/
459
/*  Calculate north direction (heading)      */
460
/*********************************************/
461
void MM3_Heading(void)
462
{
463
        int32_t sin_nick, cos_nick, sin_roll, cos_roll, sin_yaw, cos_yaw;
464
        int32_t  Hx, Hy, Hz, Hx_corr, Hy_corr;
465
        int16_t angle;
466
        int16_t heading;
467
 
468
        if (MM3_Timeout)
469
        {
470
                // Offset correction and normalization (values of H are +/- 512)
471
                Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range;
472
                Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range;
473
                Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range;
474
 
475
                // Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation
476
                // assuming the MM3 board is mounted parallel to the frame.
477
                // User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading
478
                // in a top view counter clockwise direction.
479
                // North is in opposite direction of the small arrow on the MM3 board.
480
                // Therefore 180 deg must be added to that angle.
481
                angle = ((int16_t)staticParams.UserParams2[0] + 180);
482
                // wrap angle to interval of 0°- 359°
483
                angle += 360;
484
                angle %= 360;
485
                sin_yaw = (int32_t)(c_sin_8192(angle));
486
                cos_yaw = (int32_t)(c_cos_8192(angle));
487
 
488
                Hx_corr = Hx;
489
                Hy_corr = Hy;
490
 
491
                // rotate
492
                Hx = (Hx_corr * cos_yaw - Hy_corr  * sin_yaw) / 8192;
493
                Hy = (Hx_corr * sin_yaw + Hy_corr  * cos_yaw) / 8192;
494
 
495
 
496
                // tilt compensation
497
 
498
                // calculate sinus cosinus of nick and tilt angle
499
                angle = (int16_t)(IntegralGyroNick/GYRO_DEG_FACTOR);
500
                sin_nick = (int32_t)(c_sin_8192(angle));
501
                cos_nick = (int32_t)(c_cos_8192(angle));
502
 
503
                angle = (int16_t)(IntegralGyroRoll/GYRO_DEG_FACTOR);
504
                sin_roll = (int32_t)(c_sin_8192(angle));
505
                cos_roll = (int32_t)(c_cos_8192(angle));
506
 
507
                Hx_corr = Hx * cos_nick;
508
                Hx_corr -= Hz * sin_nick;
509
                Hx_corr /= 8192;
510
 
511
                Hy_corr = Hy * cos_roll;
512
                Hy_corr += Hz * sin_roll;
513
                Hy_corr /= 8192;
514
 
515
                // calculate Heading
516
                heading = c_atan2(Hy_corr, Hx_corr);
517
 
518
                // atan returns angular range from -180 deg to 180 deg in counter clockwise notation
519
                // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
520
                if (heading < 0) heading = -heading;
521
                else heading = 360 - heading;
522
        }
523
        else // MM3_Timeout = 0 i.e now new data from external board
524
        {
525
                if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem
526
                heading = -1;
527
        }
528
        // update compass values in fc variables
529
        CompassHeading = heading;
530
        if (CompassHeading < 0) CompassOffCourse = 0;
531
        else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
532
}