Subversion Repositories FlightCtrl

Rev

Rev 782 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 782 Rev 790
Line 22... Line 22...
22
#include "mymath.h"
22
#include "mymath.h"
23
#include "fc.h"
23
#include "fc.h"
24
#include "timer0.h"
24
#include "timer0.h"
25
#include "rc.h"
25
#include "rc.h"
26
#include "eeprom.h"
26
#include "eeprom.h"
27
#include "uart.h"
27
#include "printf_P.h"
Line 28... Line 28...
28
 
28
 
Line 29... Line 29...
29
#define MAX_AXIS_VALUE          500
29
#define MAX_AXIS_VALUE          500
Line 262... Line 262...
262
/*********************************************/
262
/*********************************************/
263
/*  Calibrate Compass                        */
263
/*  Calibrate Compass                        */
264
/*********************************************/
264
/*********************************************/
265
void MM3_Calibrate(void)
265
void MM3_Calibrate(void)
266
{
266
{
-
 
267
        static uint8_t debugcounter = 0;
267
        int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0;
268
        int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0;
268
        uint8_t measurement = 50, beeper = 0;
269
        uint8_t measurement = 50, beeper = 0;
269
        uint16_t timer;
270
        uint16_t timer;
Line 270... Line 271...
270
 
271
 
271
        GRN_ON;
272
        GRN_ON;
Line 272... Line 273...
272
        ROT_OFF;
273
        ROT_OFF;
273
 
274
 
274
        // get maximum and minimum reading of all axis
275
        // get maximum and minimum reading of all axis
-
 
276
        while (measurement)
-
 
277
        {
-
 
278
                // reset range markers if yawstick ist leftmost
-
 
279
                if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 100)
-
 
280
                {
-
 
281
                        x_min = 0;
-
 
282
                        x_max = 0;
-
 
283
                        y_min = 0;
-
 
284
                        y_max = 0;
-
 
285
                        z_min = 0;
-
 
286
                        z_max = 0;
275
        while (measurement)
287
                }
276
        {
288
 
Line 277... Line 289...
277
                if (MM3.x_axis > x_max) x_max = MM3.x_axis;
289
                if (MM3.x_axis > x_max) x_max = MM3.x_axis;
278
                else if (MM3.x_axis < x_min) x_min = MM3.x_axis;
290
                else if (MM3.x_axis < x_min) x_min = MM3.x_axis;
Line 293... Line 305...
293
                beeper--;
305
                beeper--;
294
                // loop with period of 10 ms / 100 Hz
306
                // loop with period of 10 ms / 100 Hz
295
                timer = SetDelay(10);
307
                timer = SetDelay(10);
296
                while(!CheckDelay(timer));
308
                while(!CheckDelay(timer));
Line -... Line 309...
-
 
309
 
-
 
310
                if(debugcounter++ > 30)
-
 
311
                {
-
 
312
                        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);
-
 
313
                        debugcounter = 0;
-
 
314
                }
297
 
315
 
298
                // If thrust is less than 100, stop calibration with a delay of 0.5 seconds
316
                // If thrust is less than 100, stop calibration with a delay of 0.5 seconds
299
                if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--;
317
                if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--;
300
        }
318
        }
301
        // Rage of all axis
319
        // Rage of all axis
Line 328... Line 346...
328
        int32_t  Hx, Hy, Hz, Hx_corr, Hy_corr;
346
        int32_t  Hx, Hy, Hz, Hx_corr, Hy_corr;
329
        int16_t angle;
347
        int16_t angle;
330
        uint16_t div_factor;
348
        uint16_t div_factor;
331
        int16_t heading;
349
        int16_t heading;
Line 332... Line -...
332
 
-
 
333
        DebugOut.Analog[11] = MM3_Timeout;
-
 
334
 
350
 
335
        if (MM3_Timeout)
351
        if (MM3_Timeout)
336
        {
352
        {
337
                // Offset correction and normalization (values of H are +/- 512)
353
                // Offset correction and normalization (values of H are +/- 512)
338
                Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range;
354
                Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range;