Subversion Repositories FlightCtrl

Rev

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