Subversion Repositories MK3Mag

Rev

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

Rev 28 Rev 29
Line 66... Line 66...
66
#include "analog.h"
66
#include "analog.h"
67
#include "uart.h"
67
#include "uart.h"
68
 
68
 
69
 
69
 
70
AttitudeSource_t AttitudeSource = ATTITUDE_SOURCE_ACC;
70
AttitudeSource_t AttitudeSource = ATTITUDE_SOURCE_ACC;
-
 
71
Orientation_t Orientation = ORIENTATION_FC;
71
 
72
 
72
uint16_t Led_Timer = 0;
73
uint16_t Led_Timer = 0;
73
 
74
 
74
struct Scaling_t
75
struct Scaling_t
75
{
76
{
Line 83... Line 84...
83
        struct Scaling_t MagY;
84
        struct Scaling_t MagY;
84
        struct Scaling_t MagZ;
85
        struct Scaling_t MagZ;
85
        struct Scaling_t AccX;
86
        struct Scaling_t AccX;
86
        struct Scaling_t AccY;
87
        struct Scaling_t AccY;
87
        struct Scaling_t AccZ;
88
        struct Scaling_t AccZ;
88
} ;
89
};
89
 
90
 
90
struct Calibration_t eeCalibration EEMEM;       // calibration data in EEProm
91
struct Calibration_t eeCalibration EEMEM;       // calibration data in EEProm
91
struct Calibration_t Calibration;               // calibration data in RAM
92
struct Calibration_t Calibration;               // calibration data in RAM
92
 
93
 
93
// magnet sensor variable
94
// magnet sensor variable
94
int16_t RawMagnet1a, RawMagnet1b;                       // raw magnet sensor data
95
int16_t RawMagnet1a, RawMagnet1b;                       // raw magnet sensor data
95
int16_t RawMagnet2a, RawMagnet2b;
96
int16_t RawMagnet2a, RawMagnet2b;
96
int16_t RawMagnet3a, RawMagnet3b;
97
int16_t RawMagnet3a, RawMagnet3b;
97
int16_t UncalMagX, UncalMagY, UncalMagZ;        // sensor signal difference without Scaling
98
int16_t UncalMagX, UncalMagY, UncalMagZ;        // sensor signal difference without Scaling
98
int16_t MagX, MagY, MagZ;                                       // rescaled magnetic field readings
99
int16_t MagX = 0, MagY = 0, MagZ = 0;           // rescaled magnetic field readings
-
 
100
 
99
// acc sensor variables
101
// acceleration sensor variables
100
int16_t RawAccX, RawAccY, RawAccZ;                      // raw acceleration readings
102
int16_t RawAccX = 0, RawAccY = 0, RawAccZ = 0;                  // raw acceleration readings
101
int16_t AccX, AccY, AccZ;                                       // rescaled acceleration readings
103
int16_t AccX = 0, AccY = 0, AccZ = 0;                                   // rescaled acceleration readings
102
uint8_t AccPresent = 0;
-
 
103
uint8_t PC_Connected = 0;
104
int16_t AccAttitudeNick = 0, AccAttitudeRoll = 0;               // nick and roll angle from acc
104
 
105
 
105
int16_t Heading = -1;
106
int16_t Heading = -1;                                           // the current compass heading in deg
106
 
107
 
107
 
108
 
108
void CalcFields(void)
109
void CalcFields(void)
109
{
110
{
110
        UncalMagX = (RawMagnet1a - RawMagnet1b);
111
        UncalMagX = (RawMagnet1a - RawMagnet1b);
Line 115... Line 116...
115
        else MagX = 0;
116
        else MagX = 0;
116
        if(Calibration.MagY.Range != 0) MagY = (1024L * (int32_t)(UncalMagY - Calibration.MagY.Offset)) / (Calibration.MagY.Range);
117
        if(Calibration.MagY.Range != 0) MagY = (1024L * (int32_t)(UncalMagY - Calibration.MagY.Offset)) / (Calibration.MagY.Range);
117
        else MagY = 0;
118
        else MagY = 0;
118
        if(Calibration.MagY.Range != 0) MagZ = (1024L * (int32_t)(UncalMagZ - Calibration.MagZ.Offset)) / (Calibration.MagZ.Range);
119
        if(Calibration.MagY.Range != 0) MagZ = (1024L * (int32_t)(UncalMagZ - Calibration.MagZ.Offset)) / (Calibration.MagZ.Range);
119
        else MagZ = 0;
120
        else MagZ = 0;
-
 
121
 
-
 
122
        if(AccPresent)
-
 
123
        {
-
 
124
                AccX = (RawAccX - Calibration.AccX.Offset);
-
 
125
                AccY = (RawAccY - Calibration.AccY.Offset);
-
 
126
                AccZ = (Calibration.AccZ.Offset - RawAccZ);
-
 
127
 
-
 
128
                #if (BOARD == 10) // the hardware 1.0 has the LIS3L02AL
-
 
129
                // acc mode assumes orientation like FC
-
 
130
                if(AccX >  136) AccAttitudeNick = -800;
-
 
131
                else
-
 
132
                if(AccX < -136) AccAttitudeNick = 800;
-
 
133
                else                    AccAttitudeNick = (int16_t)(-1800.0 * asin((double) AccX / 138.0) / M_PI);
-
 
134
 
-
 
135
 
-
 
136
                if(AccY >  136) AccAttitudeRoll = 800;
-
 
137
                else
-
 
138
                if(AccY < -136) AccAttitudeRoll = -800;
-
 
139
                else                    AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI);
-
 
140
 
-
 
141
                #else // the hardware 1.1 has the LIS344ALH with a different axis definition (X -> -Y, Y -> X, Z -> Z)
-
 
142
                // acc mode assumes orientation like FC
-
 
143
                if(AccY >  136) AccAttitudeNick = 800;
-
 
144
                else
-
 
145
                if(AccY < -136) AccAttitudeNick = -800;
-
 
146
                else                    AccAttitudeNick = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI);
-
 
147
 
-
 
148
 
-
 
149
                if(AccX >  136) AccAttitudeRoll = 800;
-
 
150
                else
-
 
151
                if(AccX < -136) AccAttitudeRoll = -800;
-
 
152
                else                    AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccX / 138.0) / M_PI);
-
 
153
                #endif
-
 
154
        }
120
}
155
}
121
 
156
 
122
 
157
 
123
void CalcHeading(void)
158
void CalcHeading(void)
124
{
159
{
125
        double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz;
160
        double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0;
126
        int16_t heading = -1;
161
        int16_t heading = -1;
127
 
162
 
128
        // blink code for normal operation
163
        // blink code for normal operation
129
        if(CheckDelay(Led_Timer))
164
        if(CheckDelay(Led_Timer))
130
        {
165
        {
131
                LED_GRN_TOGGLE;
166
                LED_GRN_TOGGLE;
132
                Led_Timer = SetDelay(500);
167
                Led_Timer = SetDelay(500);
133
        }
168
        }
134
 
169
 
-
 
170
        switch(Orientation)
-
 
171
        {
-
 
172
                case ORIENTATION_NC:
135
        Cx = MagX;
173
                        Cx = MagX;
136
        Cy = MagY;
174
                        Cy = MagY;
137
        Cz = MagZ;
175
                        Cz = MagZ;
-
 
176
                        break;
138
 
177
 
-
 
178
                case ORIENTATION_FC:
139
        if(ExternData.Orientation == 1)
179
                        // rotation of 90 deg compared to NC setup
140
        {
-
 
141
                Cx = MagX;
180
                        Cx = MagY;
142
                Cy = -MagY;
181
                        Cy = -MagX;
143
                Cz = MagZ;
182
                        Cz = MagZ;
-
 
183
                        break;
144
        }
184
        }
145
 
185
 
146
        // calculate nick and roll angle in rad
186
        // calculate nick and roll angle in rad
147
        switch(AttitudeSource)
187
        switch(AttitudeSource)
148
        {
188
        {
Line 155... Line 195...
155
                        nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800.0);
195
                        nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800.0);
156
                        roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800.0);
196
                        roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800.0);
157
                        break;
197
                        break;
158
 
198
 
159
                case ATTITUDE_SOURCE_ACC:
199
                case ATTITUDE_SOURCE_ACC:
160
                        if(AccX >  125) nick_rad = M_PI / 2;
-
 
161
                        else
-
 
162
                        if(AccX < -125) nick_rad = -M_PI / 2;
-
 
163
                        else
-
 
164
                        {
-
 
165
                                nick_rad = asin((double) AccX / 125.0);
200
                        nick_rad = ((double)AccAttitudeNick) * M_PI / (double)(1800.0);
166
                        }
-
 
167
 
-
 
168
                        if(AccY >  125) roll_rad = M_PI / 2;
-
 
169
                        else
-
 
170
                        if(AccY < -125) roll_rad = -M_PI / 2;
-
 
171
                        else
-
 
172
                        {
-
 
173
                        roll_rad = asin((double) AccY / 125.0);
201
                        roll_rad = ((double)AccAttitudeRoll) * M_PI / (double)(1800.0);
174
                        }
-
 
175
                        break;
202
                        break;
-
 
203
 
-
 
204
                default:
-
 
205
                        nick_rad = 0;
-
 
206
                        roll_rad = 0;
-
 
207
                break;
176
        }
208
        }
177
 
209
 
178
        // calculate attitude correction
210
        // calculate attitude correction
179
        Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad);
211
        Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad);
180
        Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad);
212
        Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad);
181
 
213
 
-
 
214
        DebugOut.Analog[27] = (int16_t)Hx;
-
 
215
        DebugOut.Analog[28] = (int16_t)Hy;
-
 
216
 
-
 
217
 
182
        // calculate Heading
218
        // calculate Heading
183
        heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI);
219
        heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI);
184
        // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation
220
        // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation
185
        // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
221
        // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
186
        if (heading < 0) heading = -heading;
222
        if (heading < 0) heading = -heading;
Line 267... Line 303...
267
 
303
 
268
                case 5:
304
                case 5:
269
                        // Save values
305
                        // Save values
270
                        if(cal != calold) // avoid continously writing of eeprom!
306
                        if(cal != calold) // avoid continously writing of eeprom!
271
                        {
307
                        {
272
                                Calibration.MagY.Range = Xmax - Xmin;
308
                                Calibration.MagX.Range = Xmax - Xmin;
273
                                Calibration.MagX.Offset = (Xmin + Xmax) / 2;
309
                                Calibration.MagX.Offset = (Xmin + Xmax) / 2;
274
                                Calibration.MagY.Range = Ymax - Ymin;
310
                                Calibration.MagY.Range = Ymax - Ymin;
275
                                Calibration.MagY.Offset = (Ymin + Ymax) / 2;
311
                                Calibration.MagY.Offset = (Ymin + Ymax) / 2;
276
                                Calibration.MagZ.Range = Zmax - Zmin;
312
                                Calibration.MagZ.Range = Zmax - Zmin;
277
                                Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
313
                                Calibration.MagZ.Offset = (Zmin + Zmax) / 2;
278
                                if((Calibration.MagX.Range > 150) && (Calibration.MagY.Range > 150) && (Calibration.MagZ.Range > 150))
314
                                if((Calibration.MagX.Range > 150) && (Calibration.MagY.Range > 150) && (Calibration.MagZ.Range > 150))
279
                                {
315
                                {
280
                                        // indicate write process by setting the led
316
                                        // indicate write process by setting the led
281
                                        LED_GRN_ON;
317
                                        LED_GRN_ON;
282
                                        eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration));
318
                                        eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration));
283
                                        Delay_ms(2000);
319
                                        Led_Timer = SetDelay(2000);
284
                                        // reset led state
-
 
285
                                        LED_GRN_OFF;
-
 
286
                                        // reset  blinkcode
320
                                        // reset  blinkcode
287
                                        blinkcount = 0;
321
                                        blinkcount = 0;
288
                                        Led_Timer = SetDelay(1000);
-
 
289
                                }
322
                                }
290
                        }
323
                        }
291
                        break;
324
                        break;
292
 
325
 
293
                default:
326
                default:
Line 306... Line 339...
306
        DebugOut.Analog[4] =  UncalMagY;
339
        DebugOut.Analog[4] =  UncalMagY;
307
        DebugOut.Analog[5] =  UncalMagZ;
340
        DebugOut.Analog[5] =  UncalMagZ;
308
        switch(AttitudeSource)
341
        switch(AttitudeSource)
309
        {
342
        {
310
                case ATTITUDE_SOURCE_ACC:
343
                case ATTITUDE_SOURCE_ACC:
311
 
-
 
-
 
344
                        DebugOut.Analog[6] =  AccAttitudeNick;
-
 
345
                        DebugOut.Analog[7] =  AccAttitudeRoll;
312
                        break;
346
                        break;
313
 
347
 
314
                case ATTITUDE_SOURCE_UART:
348
                case ATTITUDE_SOURCE_UART:
315
                        DebugOut.Analog[6] =  ExternData.Attitude[NICK];
349
                        DebugOut.Analog[6] =  ExternData.Attitude[NICK];
316
                        DebugOut.Analog[7] =  ExternData.Attitude[ROLL];
350
                        DebugOut.Analog[7] =  ExternData.Attitude[ROLL];
Line 339... Line 373...
339
        DebugOut.Analog[22] = RawAccY;
373
        DebugOut.Analog[22] = RawAccY;
340
        DebugOut.Analog[23] = RawAccZ;
374
        DebugOut.Analog[23] = RawAccZ;
341
        DebugOut.Analog[24] = Calibration.AccX.Offset;
375
        DebugOut.Analog[24] = Calibration.AccX.Offset;
342
        DebugOut.Analog[25] = Calibration.AccY.Offset;
376
        DebugOut.Analog[25] = Calibration.AccY.Offset;
343
    DebugOut.Analog[26] = Calibration.AccZ.Offset;
377
    DebugOut.Analog[26] = Calibration.AccZ.Offset;
-
 
378
    DebugOut.Analog[29] = AttitudeSource;
344
}
379
}
345
 
380
 
346
void AccMeasurement(void)
381
void AccMeasurement(void)
347
{
382
{
348
        if(AccPresent)
383
        if(AccPresent)
349
        {
384
        {
350
                RawAccX = (RawAccX + (int16_t)ADC_GetValue(ADC2))/2;
385
                RawAccX = (RawAccX + (int16_t)ADC_GetValue(ACC_X))/2;
351
                RawAccY = (RawAccY + (int16_t)ADC_GetValue(ADC3))/2;
386
                RawAccY = (RawAccY + (int16_t)ADC_GetValue(ACC_Y))/2;
352
                RawAccZ = (RawAccZ + (int16_t)ADC_GetValue(ADC6))/2;
387
                RawAccZ = (RawAccZ + (int16_t)ADC_GetValue(ACC_Z))/2;
353
        }
388
        }
354
        else
389
        else
355
        {
390
        {
356
                RawAccX = 0;
391
                RawAccX = 0;
357
                RawAccY = 0;
392
                RawAccY = 0;
358
                RawAccZ = 0;
393
                RawAccZ = 0;
359
        }
394
        }
360
                AccX = ((RawAccX - Calibration.AccX.Offset) + AccX * 7) / 8;
-
 
361
                AccY = ((RawAccY - Calibration.AccY.Offset) + AccY * 7) / 8;
-
 
362
                AccZ = ((Calibration.AccZ.Offset - RawAccZ) + AccZ * 7) / 8;
-
 
363
}
395
}
364
 
396
 
365
 
-
 
366
 
-
 
367
int main (void)
397
int main (void)
368
{
398
{
369
        // reset input pullup
399
        // reset input pullup
370
        DDRC &=~((1<<DDC6));
400
        DDRC &=~((1<<DDC6));
371
        PORTC |= (1<<PORTC6);
401
        PORTC |= (1<<PORTC6);
Line 376... Line 406...
376
    ADC_Init();
406
    ADC_Init();
377
        I2C_Init();
407
        I2C_Init();
378
 
408
 
379
    sei(); // enable globale interrupts
409
    sei(); // enable globale interrupts
380
 
410
 
-
 
411
    if(AccPresent)
-
 
412
    {
-
 
413
                USART0_Print("ACC present\n");
-
 
414
        }
-
 
415
 
381
    LED_GRN_ON;
416
    LED_GRN_ON;
382
 
417
 
383
    Debug_Timer = SetDelay(200);
418
    Debug_Timer = SetDelay(200);
384
    Led_Timer = SetDelay(200);
419
    Led_Timer = SetDelay(200);
385
 
420
 
386
        // read calibration info from eeprom
421
        // read calibration info from eeprom
387
        eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration));
422
        eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration));
388
 
423
 
389
    ExternData.Orientation = 0;
-
 
390
    ExternData.CalState = 0;
424
    ExternData.CalState = 0;
391
    I2C_WriteCal.CalByte = 0;
425
    I2C_WriteCal.CalByte = 0;
392
 
426
 
393
 
427
 
394
        // main loop
428
        // main loop
395
    while (1)
429
    while (1)
396
    {
430
    {
397
                FLIP_LOW;
431
                FLIP_LOW;
398
                Delay_ms(2);
432
                Delay_ms(2);
399
                RawMagnet1a = ADC_GetValue(ADC0);
433
                RawMagnet1a = ADC_GetValue(MAG_X);
400
                RawMagnet2a = -ADC_GetValue(ADC1);
434
                RawMagnet2a = -ADC_GetValue(MAG_Y);
401
                RawMagnet3a = ADC_GetValue(ADC7);
435
                RawMagnet3a = ADC_GetValue(MAG_Z);
402
                AccMeasurement();
436
                AccMeasurement();
403
                Delay_ms(1);
437
                Delay_ms(1);
404
 
438
 
405
                FLIP_HIGH;
439
                FLIP_HIGH;
406
                Delay_ms(2);
440
                Delay_ms(2);
407
                RawMagnet1b = ADC_GetValue(ADC0);
441
                RawMagnet1b = ADC_GetValue(MAG_X);
408
                RawMagnet2b = -ADC_GetValue(ADC1);
442
                RawMagnet2b = -ADC_GetValue(MAG_Y);
409
                RawMagnet3b = ADC_GetValue(ADC7);
443
                RawMagnet3b = ADC_GetValue(MAG_Z);
410
                AccMeasurement();
444
                AccMeasurement();
411
                Delay_ms(1);
445
                Delay_ms(1);
412
 
446
 
413
                CalcFields();
447
                CalcFields();
414
 
448
 
415
                if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
449
                if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
416
                else CalcHeading();
450
                else CalcHeading();
417
 
451
 
418
                // check data from USART
452
                // check data from USART
419
        USART0_ProcessRxData();
453
        USART0_ProcessRxData();
-
 
454
 
-
 
455
                if(NC_Connected) NC_Connected--;
-
 
456
                if(FC_Connected) FC_Connected--;
-
 
457
                // fall back to attitude estimation from acc sensor if NC or FC does'nt send attittude data
-
 
458
                if(!NC_Connected && ! NC_Connected)
-
 
459
                {
-
 
460
                        AttitudeSource = ATTITUDE_SOURCE_ACC;
420
                USART0_TransmitTxData();
461
                        Orientation = ORIENTATION_FC;
-
 
462
                }
421
 
463
 
422
        if(PC_Connected)
464
        if(PC_Connected)
423
        {
465
        {
424
            USART0_EnableTXD();
466
            USART0_EnableTXD();
425
            USART0_TransmitTxData();
467
            USART0_TransmitTxData();