Subversion Repositories FlightCtrl

Rev

Rev 1539 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1538 killagreg 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 and I. Busker are under the license (License.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
}