Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
700 killagreg 1
/*
2
 
741 killagreg 3
Copyright 2008, by Killagreg
700 killagreg 4
 
741 killagreg 5
This program (files mm3.c and mm3.h) is free software; you can redistribute it and/or modify
700 killagreg 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
 
741 killagreg 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
700 killagreg 15
*/
16
#include <stdlib.h>
17
#include <avr/io.h>
18
#include <avr/interrupt.h>
19
 
20
#include "mm3.h"
21
#include "main.h"
22
#include "mymath.h"
23
#include "fc.h"
24
#include "timer0.h"
25
#include "rc.h"
26
#include "eeprom.h"
27
 
28
#define MAX_AXIS_VALUE          500
29
 
30
 
31
typedef struct
32
{
33
        uint8_t STATE;
34
        uint16_t DRDY;
35
        uint8_t AXIS;
36
        int16_t x_axis;
37
        int16_t y_axis;
38
        int16_t z_axis;
39
} MM3_working_t;
40
 
41
 
42
// MM3 State Machine
43
#define MM3_STATE_RESET                         0
44
#define MM3_STATE_START_TRANSFER        1
45
#define MM3_STATE_WAIT_DRDY                     2
46
#define MM3_STATE_DRDY                          3
47
#define MM3_STATE_BYTE2                         4
48
 
49
#define MM3_X_AXIS              0x01
50
#define MM3_Y_AXIS              0x02
51
#define MM3_Z_AXIS              0x03
52
 
53
 
54
#define MM3_PERIOD_32   0x00
55
#define MM3_PERIOD_64   0x10
56
#define MM3_PERIOD_128  0x20
57
#define MM3_PERIOD_256  0x30
58
#define MM3_PERIOD_512  0x40
59
#define MM3_PERIOD_1024 0x50
60
#define MM3_PERIOD_2048 0x60
61
#define MM3_PERIOD_4096 0x70
62
 
63
MM3_calib_t MM3_calib;
64
volatile MM3_working_t MM3;
754 killagreg 65
volatile uint8_t MM3_Timeout = 0;
700 killagreg 66
 
67
 
68
 
69
/*********************************************/
70
/*  Initialize Interface to MM3 Compass      */
71
/*********************************************/
726 killagreg 72
void MM3_Init(void)
700 killagreg 73
{
74
        uint8_t sreg = SREG;
75
 
76
        cli();
77
 
78
        // Configure Pins for SPI
79
        // set SCK (PB7), MOSI (PB5) as output
80
        DDRB |= (1<<DDB7)|(1<<DDB5);
81
        // set MISO (PB6) as input
82
        DDRB &= ~(1<<DDB6);
83
 
84
        // Output Pins PC4->MM3_SS ,PC5->MM3_RESET
85
        DDRC |= (1<<DDC4)|(1<<DDC5);
86
        // set pins permanent to low
87
        PORTC &= ~((1<<PORTC4)|(1<<PORTC5));
88
 
89
        // Initialize SPI-Interface
90
        // Enable interrupt (SPIE=1)
91
        // Enable SPI bus (SPE=1)
92
        // MSB transmitted first (DORD = 0)
93
        // Master SPI Mode (MSTR=1)
94
        // Clock polarity low whn idle (CPOL=0)
95
        // clock phase sample at leading edge (CPHA=0)
96
        // clock rate = SYSCLK/128 (SPI2X=0, SPR1=1, SPR0=1) 20MHz/128 = 156.25kHz
97
        SPCR = (1<<SPIE)|(1<<SPE)|(0<<DORD)|(1<<MSTR)|(0<<CPOL)|(0<<CPHA)|(1<<SPR1)|(1<<SPR0);
98
        SPSR &= ~(1<<SPI2X);
99
 
100
    // Init Statemachine
101
        MM3.AXIS = MM3_X_AXIS;
102
        MM3.STATE = MM3_STATE_RESET;
103
 
104
        // Read calibration from EEprom
105
        MM3_calib.X_off = (int8_t)GetParamByte(PID_MM3_X_OFF);
106
        MM3_calib.Y_off = (int8_t)GetParamByte(PID_MM3_Y_OFF);
107
        MM3_calib.Z_off = (int8_t)GetParamByte(PID_MM3_Z_OFF);
108
        MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE);
109
        MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE);
110
        MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE);
111
 
741 killagreg 112
        MM3_Timeout = 0;
113
 
700 killagreg 114
        SREG = sreg;
115
}
116
 
117
 
118
/*********************************************/
119
/*  Get Data from MM3                        */
120
/*********************************************/
754 killagreg 121
void MM3_Update(void) // called every 102.4 µs by timer 0 ISR
700 killagreg 122
{
123
        switch (MM3.STATE)
124
        {
125
        case MM3_STATE_RESET:
741 killagreg 126
                PORTC &= ~(1<<PORTC4);  // select slave
700 killagreg 127
                PORTC |= (1<<PORTC5);   // PC5 to High, MM3 Reset
128
                MM3.STATE = MM3_STATE_START_TRANSFER;
129
                return;
130
 
131
        case MM3_STATE_START_TRANSFER:
132
                PORTC &= ~(1<<PORTC5);  // PC4 auf Low (was 102.4 µs at high level)
133
 
134
                // write to SPDR triggers automatically the transfer MOSI MISO
135
                // MM3 Period, + AXIS code
741 killagreg 136
                switch(MM3.AXIS)
137
                {
138
                case MM3_X_AXIS:
139
                        SPDR = MM3_PERIOD_256 + MM3_X_AXIS;
140
                        break;
141
                case MM3_Y_AXIS:
142
                        SPDR = MM3_PERIOD_256 + MM3_Y_AXIS;
143
                        break;
144
                case MM3_Z_AXIS:
145
                        SPDR = MM3_PERIOD_256 + MM3_Z_AXIS;
146
                        break;
147
                default:
148
                        MM3.AXIS = MM3_X_AXIS;
149
                        MM3.STATE = MM3_STATE_RESET;
150
                        return;
151
                }
700 killagreg 152
 
153
                // DRDY line is not connected, therefore
154
                // wait before reading data back
155
                MM3.DRDY = SetDelay(8); // wait 8ms for data ready
156
                MM3.STATE = MM3_STATE_WAIT_DRDY;
157
                return;
158
 
159
        case MM3_STATE_WAIT_DRDY:
160
                if (CheckDelay(MM3.DRDY))
161
                {
162
                        // write something into SPDR to trigger data reading
163
                        SPDR = 0x00;
164
                        MM3.STATE = MM3_STATE_DRDY;
165
                }
166
                return;
167
        }
168
}
169
 
170
 
171
/*********************************************/
172
/*  Interrupt SPI transfer complete          */
173
/*********************************************/
174
ISR(SPI_STC_vect)
175
{
176
        static int8_t tmp;
177
        int16_t value;
178
 
179
        switch (MM3.STATE)
180
        {
181
        // 1st byte received
182
        case MM3_STATE_DRDY:
183
                tmp = SPDR;     // store 1st byte
184
                SPDR = 0x00;    // trigger transfer of 2nd byte
185
                MM3.STATE = MM3_STATE_BYTE2;
186
                return;
187
 
188
        case MM3_STATE_BYTE2:           // 2nd byte received
189
                value = (int16_t)tmp;   // combine the 1st and 2nd byte to a word
190
                value <<= 8;            // shift 1st byte to MSB-Position
191
                value |= (int16_t)SPDR; // add 2nd byte
192
 
193
                if(abs(value) < MAX_AXIS_VALUE)         // ignore spikes
194
                {
195
                        switch (MM3.AXIS)
196
                        {
197
                        case MM3_X_AXIS:
198
                                MM3.x_axis = value;
199
                                MM3.AXIS = MM3_Y_AXIS;
200
                                break;
201
                        case MM3_Y_AXIS:
202
                                MM3.y_axis = value;
203
                                MM3.AXIS = MM3_Z_AXIS;
204
                                break;
205
                        case MM3_Z_AXIS:
206
                                MM3.z_axis = value;
207
                                MM3.AXIS = MM3_X_AXIS;
208
                                break;
209
                        default:
210
                                MM3.AXIS = MM3_X_AXIS;
211
                                break;
212
                        }
213
                }
727 killagreg 214
                PORTC |= (1<<PORTC4); // deselect slave
700 killagreg 215
                MM3.STATE = MM3_STATE_RESET;
741 killagreg 216
                // Update timeout is called every 102.4 µs.
217
                // It takes 2 cycles to write a measurement data request for one axis and
218
                // at at least 8 ms / 102.4 µs = 79 cycles to read the requested data back.
219
                // I.e. 81 cycles * 102.4 µs = 8.3ms per axis.
220
                // The two function accessing the MM3 Data - MM3_Calibrate() and MM3_Heading() -
221
                // decremtent the MM3_Timeout every 100 ms.
222
                // incrementing the counter by 1 every 8.3 ms is sufficient to avoid a timeout.
223
                if ((MM3.x_axis != MM3.y_axis) || (MM3.x_axis != MM3.z_axis) || (MM3.y_axis != MM3.z_axis))
224
                {       // if all axis measurements give diffrent readings the data should be valid
225
                        if(MM3_Timeout < 20) MM3_Timeout++;
226
                }
227
                else // something is very strange here
228
                {
229
                        if(MM3_Timeout ) MM3_Timeout--;
230
                }
231
                return;
232
 
233
        default:
234
                return;
700 killagreg 235
        }
236
}
237
 
238
 
239
 
240
/*********************************************/
241
/*  Calibrate Compass                        */
242
/*********************************************/
726 killagreg 243
void MM3_Calibrate(void)
700 killagreg 244
{
245
        int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0;
246
        uint8_t measurement = 50, beeper = 0;
247
        uint16_t timer;
248
 
249
        GRN_ON;
250
        ROT_OFF;
251
 
252
        // get maximum and minimum reading of all axis
773 killagreg 253
        while (measurement)
700 killagreg 254
        {
255
                if (MM3.x_axis > x_max) x_max = MM3.x_axis;
256
                else if (MM3.x_axis < x_min) x_min = MM3.x_axis;
257
 
258
                if (MM3.y_axis > y_max) y_max = MM3.y_axis;
259
                else if (MM3.y_axis < y_min) y_min = MM3.y_axis;
260
 
261
                if (MM3.z_axis > z_max) z_max = MM3.z_axis;
262
                else if (MM3.z_axis < z_min) z_min = MM3.z_axis;
263
 
264
                if (!beeper)
265
                {
266
                        ROT_FLASH;
267
                        GRN_FLASH;
268
                        BeepTime = 50;
269
                        beeper = 50;
270
                }
271
                beeper--;
272
                // loop with period of 10 ms / 100 Hz
273
                timer = SetDelay(10);
274
                while(!CheckDelay(timer));
275
 
707 killagreg 276
                // If thrust is less than 100, stop calibration with a delay of 0.5 seconds
277
                if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--;
700 killagreg 278
        }
773 killagreg 279
        // Rage of all axis
280
        MM3_calib.X_range = (x_max - x_min);
281
        MM3_calib.Y_range = (y_max - y_min);
282
        MM3_calib.Z_range = (z_max - z_min);
700 killagreg 283
 
773 killagreg 284
        // Offset of all axis
285
        MM3_calib.X_off = (x_max + x_min) / 2;
286
        MM3_calib.Y_off = (y_max + y_min) / 2;
287
        MM3_calib.Z_off = (z_max + z_min) / 2;
700 killagreg 288
 
773 killagreg 289
        // save to EEProm
290
        SetParamByte(PID_MM3_X_OFF,   (uint8_t)MM3_calib.X_off);
291
        SetParamByte(PID_MM3_Y_OFF,   (uint8_t)MM3_calib.Y_off);
292
        SetParamByte(PID_MM3_Z_OFF,   (uint8_t)MM3_calib.Z_off);
293
        SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range);
294
        SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range);
295
        SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range);
296
 
700 killagreg 297
}
298
 
299
 
300
/*********************************************/
301
/*  Calculate north direction (heading)      */
302
/*********************************************/
726 killagreg 303
int16_t MM3_Heading(void)
700 killagreg 304
{
701 killagreg 305
        int32_t sin_pitch, cos_pitch, sin_roll, cos_roll, sin_yaw, cos_yaw;
700 killagreg 306
        int32_t  Hx, Hy, Hz, Hx_corr, Hy_corr;
307
        int16_t angle;
308
        uint16_t div_factor;
309
        int16_t heading;
310
 
741 killagreg 311
        if (MM3_Timeout)
312
        {
313
                // Offset correction and normalization (values of H are +/- 512)
314
                Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range;
315
                Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range;
316
                Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range;
700 killagreg 317
 
741 killagreg 318
                // Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation
319
                // assuming the MM3 board is mounted parallel to the frame.
320
                // User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading
321
                // in a top view counter clockwise direction.
322
                // North is in opposite direction of the small arrow on the MM3 board.
323
                // Therefore 180 deg must be added to that angle.
324
                angle = ((int16_t)ParamSet.UserParam4 + 180);
325
                // wrap angle to interval of 0°- 359°
326
                angle += 360;
327
                angle %= 360;
328
                sin_yaw = (int32_t)(c_sin_8192(angle));
329
                cos_yaw = (int32_t)(c_cos_8192(angle));
700 killagreg 330
 
741 killagreg 331
                Hx_corr = Hx;
332
                Hy_corr = Hy;
700 killagreg 333
 
741 killagreg 334
                // rotate
335
                Hx = (Hx_corr * cos_yaw - Hy_corr  * sin_yaw) / 8192;
336
                Hy = (Hx_corr * sin_yaw + Hy_corr  * cos_yaw) / 8192;
700 killagreg 337
 
338
 
741 killagreg 339
                // tilt compensation
700 killagreg 340
 
741 killagreg 341
                // calibration factor for transforming Gyro Integrals to angular degrees
342
                div_factor = (uint16_t)ParamSet.UserParam3 * 8;
700 killagreg 343
 
741 killagreg 344
                // calculate sinus cosinus of pitch and tilt angle
345
                angle = (IntegralPitch/div_factor);
346
                sin_pitch = (int32_t)(c_sin_8192(angle));
347
                cos_pitch = (int32_t)(c_cos_8192(angle));
700 killagreg 348
 
741 killagreg 349
                angle = (IntegralRoll/div_factor);
350
                sin_roll = (int32_t)(c_sin_8192(angle));
351
                cos_roll = (int32_t)(c_cos_8192(angle));
700 killagreg 352
 
741 killagreg 353
                Hx_corr = Hx * cos_pitch;
354
                Hx_corr -= Hz * sin_pitch;
355
                Hx_corr /= 8192;
700 killagreg 356
 
741 killagreg 357
                Hy_corr = Hy * cos_roll;
358
                Hy_corr += Hz * sin_roll;
359
                Hy_corr /= 8192;
700 killagreg 360
 
741 killagreg 361
                // calculate Heading
362
                heading = c_atan2(Hy_corr, Hx_corr);
700 killagreg 363
 
741 killagreg 364
                // atan returns angular range from -180 deg to 180 deg in counter clockwise notation
365
                // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
366
                if (heading < 0) heading = -heading;
367
                else heading = 360 - heading;
368
        }
369
        else // MM3_Timeout = 0 i.e now new data from external board
370
        {
765 killagreg 371
                if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem
741 killagreg 372
                heading = -1;
373
        }
700 killagreg 374
return heading;
375
}