Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

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