Subversion Repositories FlightCtrl

Rev

Rev 727 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 727 Rev 741
Line 1... Line 1...
1
/*
1
/*
Line 2... Line 2...
2
 
2
 
Line 3... Line 3...
3
Copyright 2007, Niklas Nold
3
Copyright 2008, by Killagreg
4
 
4
 
5
This program (files compass.c and compass.h) is free software; you can redistribute it and/or modify
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;
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.
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;
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
9
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
Line -... Line 10...
-
 
10
GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License
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/>.
11
along with this program. If not, see <http://www.gnu.org/licenses/>.
12
 
12
 
13
Please note: The original implementation was done by Niklas Nold.
13
Please note: All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de
14
All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de
14
*/
15
*/
Line 59... Line 60...
59
#define MM3_PERIOD_2048 0x60
60
#define MM3_PERIOD_2048 0x60
60
#define MM3_PERIOD_4096 0x70
61
#define MM3_PERIOD_4096 0x70
Line 61... Line 62...
61
 
62
 
62
MM3_calib_t MM3_calib;
63
MM3_calib_t MM3_calib;
-
 
64
volatile MM3_working_t MM3;
Line 63... Line 65...
63
volatile MM3_working_t MM3;
65
static volatile uint8_t MM3_Timeout = 0;
64
 
66
 
Line 105... Line 107...
105
        MM3_calib.Z_off = (int8_t)GetParamByte(PID_MM3_Z_OFF);
107
        MM3_calib.Z_off = (int8_t)GetParamByte(PID_MM3_Z_OFF);
106
        MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE);
108
        MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE);
107
        MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE);
109
        MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE);
108
        MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE);
110
        MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE);
Line -... Line 111...
-
 
111
 
-
 
112
        MM3_Timeout = 0;
109
 
113
 
110
        SREG = sreg;
114
        SREG = sreg;
Line 111... Line 115...
111
}
115
}
112
 
116
 
113
 
117
 
114
/*********************************************/
118
/*********************************************/
115
/*  Get Data from MM3                        */
119
/*  Get Data from MM3                        */
116
/*********************************************/
120
/*********************************************/
117
void MM3_Update() // called every 102.4 ms by timer 0 ISR
121
void MM3_Update() // called every 102.4 µs by timer 0 ISR
118
{
122
{
119
        switch (MM3.STATE)
123
        switch (MM3.STATE)
120
        {
124
        {
121
        case MM3_STATE_RESET:
125
        case MM3_STATE_RESET:
122
                PORTC &= ~(1<<PORTC4); // select slave
126
                PORTC &= ~(1<<PORTC4);  // select slave
Line 123... Line 127...
123
                PORTC |= (1<<PORTC5);   // PC5 to High, MM3 Reset
127
                PORTC |= (1<<PORTC5);   // PC5 to High, MM3 Reset
124
                MM3.STATE = MM3_STATE_START_TRANSFER;
128
                MM3.STATE = MM3_STATE_START_TRANSFER;
Line 125... Line 129...
125
                return;
129
                return;
126
 
130
 
-
 
131
        case MM3_STATE_START_TRANSFER:
-
 
132
                PORTC &= ~(1<<PORTC5);  // PC4 auf Low (was 102.4 µs at high level)
-
 
133
 
127
        case MM3_STATE_START_TRANSFER:
134
                // write to SPDR triggers automatically the transfer MOSI MISO
-
 
135
                // MM3 Period, + AXIS code
-
 
136
                switch(MM3.AXIS)
128
                PORTC &= ~(1<<PORTC5);  // PC4 auf Low (was 102.4 µs at high level)
137
                {
-
 
138
                case MM3_X_AXIS:
-
 
139
                        SPDR = MM3_PERIOD_256 + MM3_X_AXIS;
129
 
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;
Line 130... Line 147...
130
                // write to SPDR triggers automatically the transfer MOSI MISO
147
                default:
131
                // MM3 Period, + AXIS code
148
                        MM3.AXIS = MM3_X_AXIS;
132
                if (MM3.AXIS == MM3_X_AXIS) SPDR = MM3_PERIOD_256 + MM3_X_AXIS;
149
                        MM3.STATE = MM3_STATE_RESET;
133
                else if (MM3.AXIS == MM3_Y_AXIS) SPDR = MM3_PERIOD_256 + MM3_Y_AXIS;
150
                        return;
Line 194... Line 211...
194
                                break;
211
                                break;
195
                        }
212
                        }
196
                }
213
                }
197
                PORTC |= (1<<PORTC4); // deselect slave
214
                PORTC |= (1<<PORTC4); // deselect slave
198
                MM3.STATE = MM3_STATE_RESET;
215
                MM3.STATE = MM3_STATE_RESET;
-
 
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;
199
        }
235
        }
200
}
236
}
Line 212... Line 248...
212
 
248
 
213
        GRN_ON;
249
        GRN_ON;
Line 214... Line 250...
214
        ROT_OFF;
250
        ROT_OFF;
215
 
251
 
216
        // get maximum and minimum reading of all axis
252
        // get maximum and minimum reading of all axis
217
        while (measurement)
253
        while (measurement && !MM3_Timeout)
218
        {
254
        {
Line 219... Line 255...
219
                if (MM3.x_axis > x_max) x_max = MM3.x_axis;
255
                if (MM3.x_axis > x_max) x_max = MM3.x_axis;
Line 231... Line 267...
231
                        GRN_FLASH;
267
                        GRN_FLASH;
232
                        BeepTime = 50;
268
                        BeepTime = 50;
233
                        beeper = 50;
269
                        beeper = 50;
234
                }
270
                }
235
                beeper--;
271
                beeper--;
236
 
-
 
237
                // loop with period of 10 ms / 100 Hz
272
                // loop with period of 10 ms / 100 Hz
238
                timer = SetDelay(10);
273
                timer = SetDelay(10);
239
                while(!CheckDelay(timer));
274
                while(!CheckDelay(timer));
Line 240... Line 275...
240
 
275
 
241
                // If thrust is less than 100, stop calibration with a delay of 0.5 seconds
276
                // If thrust is less than 100, stop calibration with a delay of 0.5 seconds
242
                if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--;
277
                if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--;
-
 
278
        }
243
        }
279
        if(!MM3_Timeout)
244
 
280
        {
245
        // Rage of all axis
281
                // Rage of all axis
246
        MM3_calib.X_range = (x_max - x_min);
282
                MM3_calib.X_range = (x_max - x_min);
247
        MM3_calib.Y_range = (y_max - y_min);
283
                MM3_calib.Y_range = (y_max - y_min);
248
        MM3_calib.Z_range = (z_max - z_min);
284
                MM3_calib.Z_range = (z_max - z_min);
249
 
285
 
250
        // Offset of all axis
286
                // Offset of all axis
251
        MM3_calib.X_off = (x_max + x_min) / 2;
287
                MM3_calib.X_off = (x_max + x_min) / 2;
252
        MM3_calib.Y_off = (y_max + y_min) / 2;
288
                MM3_calib.Y_off = (y_max + y_min) / 2;
253
        MM3_calib.Z_off = (z_max + z_min) / 2;
289
                MM3_calib.Z_off = (z_max + z_min) / 2;
254
 
290
 
255
        // save to EEProm
291
                // save to EEProm
256
        SetParamByte(PID_MM3_X_OFF,   (uint8_t)MM3_calib.X_off);
292
                SetParamByte(PID_MM3_X_OFF,   (uint8_t)MM3_calib.X_off);
257
        SetParamByte(PID_MM3_Y_OFF,   (uint8_t)MM3_calib.Y_off);
293
                SetParamByte(PID_MM3_Y_OFF,   (uint8_t)MM3_calib.Y_off);
258
        SetParamByte(PID_MM3_Z_OFF,   (uint8_t)MM3_calib.Z_off);
294
                SetParamByte(PID_MM3_Z_OFF,   (uint8_t)MM3_calib.Z_off);
259
        SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range);
295
                SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range);
260
        SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range);
296
                SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range);
-
 
297
                SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range);
261
        SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range);
298
        }
Line 262... Line 299...
262
}
299
}
263
 
300
 
Line 271... Line 308...
271
        int32_t  Hx, Hy, Hz, Hx_corr, Hy_corr;
308
        int32_t  Hx, Hy, Hz, Hx_corr, Hy_corr;
272
        int16_t angle;
309
        int16_t angle;
273
        uint16_t div_factor;
310
        uint16_t div_factor;
274
        int16_t heading;
311
        int16_t heading;
Line 275... Line -...
275
 
-
 
276
        // calibration factor for transforming Gyro Integrals to angular degrees
-
 
277
        div_factor = (uint16_t)ParamSet.UserParam3 * 8;
-
 
278
 
-
 
279
        // Offset correction and normalization (values of H are +/- 512)
-
 
280
        Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range;
-
 
281
        Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range;
-
 
282
        Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range;
-
 
283
 
-
 
284
        // Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation
-
 
285
    // assuming the MM3 board is mounted parallel to the frame.
-
 
286
    // User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading
-
 
287
    // in a top view counter clockwise direction.
-
 
288
    // North is in opposite direction of the small arrow on the MM3 board.
-
 
289
    // Therefore 180 deg must be added to that angle.
-
 
290
        angle = ((int16_t)ParamSet.UserParam4 + 180);
-
 
291
        // wrap angle to interval of 0°- 359°
-
 
292
        angle += 360;
-
 
293
        angle %= 360;
-
 
294
        sin_yaw = (int32_t)(c_sin_8192(angle));
-
 
295
        cos_yaw = (int32_t)(c_cos_8192(angle));
-
 
296
 
-
 
297
        Hx_corr = Hx;
-
 
298
        Hy_corr = Hy;
-
 
299
 
-
 
300
        // rotate
-
 
301
        Hx = (Hx_corr * cos_yaw - Hy_corr  * sin_yaw) / 8192;
-
 
302
        Hy = (Hx_corr * sin_yaw + Hy_corr  * cos_yaw) / 8192;
-
 
303
 
-
 
304
 
312
 
305
    // tilt compensation
313
        if (MM3_Timeout)
306
 
314
        {
307
        // calibration factor for transforming Gyro Integrals to angular degrees
315
                // calibration factor for transforming Gyro Integrals to angular degrees
308
        div_factor = (uint16_t)ParamSet.UserParam3 * 8;
-
 
309
 
-
 
310
        // calculate sinus cosinus of pitch and tilt angle
-
 
311
        angle = (IntegralPitch/div_factor);
-
 
312
        sin_pitch = (int32_t)(c_sin_8192(angle));
-
 
313
        cos_pitch = (int32_t)(c_cos_8192(angle));
-
 
314
 
-
 
315
        angle = (IntegralRoll/div_factor);
-
 
316
        sin_roll = (int32_t)(c_sin_8192(angle));
-
 
317
        cos_roll = (int32_t)(c_cos_8192(angle));
-
 
318
 
-
 
319
        Hx_corr = Hx * cos_pitch;
-
 
320
        Hx_corr -= Hz * sin_pitch;
-
 
321
        Hx_corr /= 8192;
-
 
322
 
-
 
323
        Hy_corr = Hy * cos_roll;
-
 
324
        Hy_corr += Hz * sin_roll;
-
 
325
        Hy_corr /= 8192;
-
 
326
 
-
 
327
        // calculate Heading
-
 
328
        heading = c_atan2(Hy_corr, Hx_corr);
-
 
329
 
-
 
330
    // atan returns angular range from -180 deg to 180 deg in counter clockwise notation
-
 
331
    // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
-
 
332
        if (heading < 0) heading = -heading;
-
 
Line -... Line 316...
-
 
316
                div_factor = (uint16_t)ParamSet.UserParam3 * 8;
-
 
317
 
-
 
318
                // Offset correction and normalization (values of H are +/- 512)
-
 
319
                Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range;
-
 
320
                Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range;
-
 
321
                Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range;
-
 
322
 
-
 
323
                // Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation
-
 
324
                // assuming the MM3 board is mounted parallel to the frame.
-
 
325
                // User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading
-
 
326
                // in a top view counter clockwise direction.
-
 
327
                // North is in opposite direction of the small arrow on the MM3 board.
-
 
328
                // Therefore 180 deg must be added to that angle.
-
 
329
                angle = ((int16_t)ParamSet.UserParam4 + 180);
-
 
330
                // wrap angle to interval of 0°- 359°
-
 
331
                angle += 360;
-
 
332
                angle %= 360;
-
 
333
                sin_yaw = (int32_t)(c_sin_8192(angle));
-
 
334
                cos_yaw = (int32_t)(c_cos_8192(angle));
-
 
335
 
-
 
336
                Hx_corr = Hx;
-
 
337
                Hy_corr = Hy;
-
 
338
 
-
 
339
                // rotate
-
 
340
                Hx = (Hx_corr * cos_yaw - Hy_corr  * sin_yaw) / 8192;
-
 
341
                Hy = (Hx_corr * sin_yaw + Hy_corr  * cos_yaw) / 8192;
-
 
342
 
-
 
343
 
-
 
344
                // tilt compensation
-
 
345
 
-
 
346
                // calibration factor for transforming Gyro Integrals to angular degrees
-
 
347
                div_factor = (uint16_t)ParamSet.UserParam3 * 8;
-
 
348
 
-
 
349
                // calculate sinus cosinus of pitch and tilt angle
-
 
350
                angle = (IntegralPitch/div_factor);
-
 
351
                sin_pitch = (int32_t)(c_sin_8192(angle));
-
 
352
                cos_pitch = (int32_t)(c_cos_8192(angle));
-
 
353
 
-
 
354
                angle = (IntegralRoll/div_factor);
-
 
355
                sin_roll = (int32_t)(c_sin_8192(angle));
-
 
356
                cos_roll = (int32_t)(c_cos_8192(angle));
-
 
357
 
-
 
358
                Hx_corr = Hx * cos_pitch;
-
 
359
                Hx_corr -= Hz * sin_pitch;
-
 
360
                Hx_corr /= 8192;
-
 
361
 
-
 
362
                Hy_corr = Hy * cos_roll;
-
 
363
                Hy_corr += Hz * sin_roll;
-
 
364
                Hy_corr /= 8192;
-
 
365
 
-
 
366
                // calculate Heading
-
 
367
                heading = c_atan2(Hy_corr, Hx_corr);
-
 
368
 
-
 
369
                // atan returns angular range from -180 deg to 180 deg in counter clockwise notation
-
 
370
                // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
-
 
371
                if (heading < 0) heading = -heading;
-
 
372
                else heading = 360 - heading;
-
 
373
        }
-
 
374
        else // MM3_Timeout = 0 i.e now new data from external board
-
 
375
        {
333
        else heading = 360 - heading;
376
                heading = -1;
334
 
377
        }