Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

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