Subversion Repositories MK3Mag

Rev

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

Rev 21 Rev 22
Line 89... Line 89...
89
Calibration_t eeCalibration EEMEM; // calibration data in EEProm
89
Calibration_t eeCalibration EEMEM; // calibration data in EEProm
90
Calibration_t Calibration;         // calibration data in RAM
90
Calibration_t Calibration;         // calibration data in RAM
91
 
91
 
92
 
92
 
93
int16_t UncalMagnetX, UncalMagnetY, UncalMagnetZ;       // sensor signal difference without Scaling
93
int16_t UncalMagnetX, UncalMagnetY, UncalMagnetZ;       // sensor signal difference without Scaling
94
int16_t MagnetX, MagnetY, MagnetZ;                                      // rescaled magnetic field readings
94
int16_t MagnetX, MagnetY, MagnetZ;                      // rescaled magnetic field readings
95
 
95
 
96
uint8_t PC_Connected = 0;
96
uint8_t PC_Connected = 0;
97
 
97
 
98
int16_t Heading = -1;
98
int16_t Heading = -1;
99
 
99
 
Line 102... Line 102...
102
{
102
{
103
        UncalMagnetX = (1 * UncalMagnetX + (RawMagnet1a - RawMagnet1b)) / 2;
103
        UncalMagnetX = (1 * UncalMagnetX + (RawMagnet1a - RawMagnet1b)) / 2;
104
        UncalMagnetY = (1 * UncalMagnetY + (RawMagnet3a - RawMagnet3b)) / 2;
104
        UncalMagnetY = (1 * UncalMagnetY + (RawMagnet3a - RawMagnet3b)) / 2;
105
        UncalMagnetZ = (1 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 2;
105
        UncalMagnetZ = (1 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 2;
106
 
106
 
107
        MagnetX = (1024L * (int32_t)(UncalMagnetX - Calibration.X.Offset)) / (Calibration.X.Range);
107
        if(Calibration.X.Range != 0) MagnetX = (1024L * (int32_t)(UncalMagnetX - Calibration.X.Offset)) / (Calibration.X.Range);
-
 
108
        else MagnetX = 0;
108
        MagnetY = (1024L * (int32_t)(UncalMagnetY - Calibration.Y.Offset)) / (Calibration.Y.Range);
109
        if(Calibration.Y.Range != 0) MagnetY = (1024L * (int32_t)(UncalMagnetY - Calibration.Y.Offset)) / (Calibration.Y.Range);
-
 
110
        else MagnetY = 0;
109
        MagnetZ = (1024L * (int32_t)(UncalMagnetZ - Calibration.Z.Offset)) / (Calibration.Z.Range);
111
        if(Calibration.Y.Range != 0) MagnetZ = (1024L * (int32_t)(UncalMagnetZ - Calibration.Z.Offset)) / (Calibration.Z.Range);
-
 
112
        else MagnetZ = 0;
110
}
113
}
111
 
114
 
112
 
115
 
113
void CalcHeading(void)
116
void CalcHeading(void)
114
{
117
{
Line 157... Line 160...
157
{
160
{
158
        uint8_t cal;
161
        uint8_t cal;
159
        static uint8_t calold = 0;
162
        static uint8_t calold = 0;
160
        static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
163
        static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
161
        static uint8_t blinkcount = 0;
164
        static uint8_t blinkcount = 0;
162
/*
165
 
163
        // check both sources of communication for calibration request
166
        // check both sources of communication for calibration request
164
        if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;
167
        if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;
165
        else                     cal = ExternData.CalState;*/
168
        else                     cal = ExternData.CalState;
166
 
-
 
167
        cal = ExternData.CalState;
-
 
168
 
169
 
-
 
170
        DebugOut.Analog[30] = cal;
169
        // blink code for current calibration state
171
        // blink code for current calibration state
170
        if(cal)
172
        if(cal)
171
        {
173
        {
172
                if(CheckDelay(Led_Timer) || (cal != calold))
174
                if(CheckDelay(Led_Timer) || (cal != calold))
173
                {
175
                {
Line 278... Line 280...
278
        DebugOut.Analog[13] = Calibration.Z.Range;
280
        DebugOut.Analog[13] = Calibration.Z.Range;
279
        DebugOut.Analog[14] = ExternData.CalState;
281
        DebugOut.Analog[14] = ExternData.CalState;
280
        DebugOut.Analog[15] = Heading;
282
        DebugOut.Analog[15] = Heading;
281
        DebugOut.Analog[16] = ExternData.UserParam[0];
283
        DebugOut.Analog[16] = ExternData.UserParam[0];
282
        DebugOut.Analog[17] = ExternData.UserParam[1];
284
        DebugOut.Analog[17] = ExternData.UserParam[1];
-
 
285
        DebugOut.Analog[30] = I2C_WriteCal.CalByte;
283
        DebugOut.Analog[31] = PC_Connected;
286
        DebugOut.Analog[31] = PC_Connected;
284
}
287
}
285
 
288
 
286
 
289
 
287
int main (void)
290
int main (void)
288
{
291
{
-
 
292
        // reset input pullup
-
 
293
        DDRC &=~((1<<DDC6));
289
    Led_Init();
294
        PORTC |= (1<<PORTC6);
-
 
295
 
290
    LED_GRN_ON;
296
    LED_Init();
291
    TIMER0_Init();
297
    TIMER0_Init();
292
    USART0_Init();
298
    USART0_Init();
293
    ADC_Init();
299
    ADC_Init();
294
        I2C_Init();
300
        I2C_Init();
295
 
301
 
-
 
302
    sei(); // enable globale interrupts
296
 
303
 
297
    sei(); //Globale Interrupts Einschalten
304
    LED_GRN_ON;
298
 
-
 
299
 
-
 
300
 
305
 
301
    Debug_Timer = SetDelay(100);   // Sendeintervall
306
    Debug_Timer = SetDelay(200);
302
    Led_Timer = SetDelay(100);
307
    Led_Timer = SetDelay(200);
303
 
308
 
304
        // read calibration info from eeprom
309
        // read calibration info from eeprom
305
        eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration_t));
310
        eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration_t));
306
 
311
 
307
    ExternData.Orientation = 0;
312
    ExternData.Orientation = 0;
308
    ExternData.CalState = 0;
313
    ExternData.CalState = 0;
309
    I2C_WriteCal.CalByte = 0;
314
    I2C_WriteCal.CalByte = 0;
310
 
315
 
-
 
316
        Heading = 101;
-
 
317
 
311
        // main loop
318
        // main loop
312
    while (1)
319
    while (1)
313
    {
320
    {
-
 
321
 
-
 
322
 
314
                FLIP_LOW;
323
                FLIP_LOW;
315
                Delay_ms(2);
324
                Delay_ms(2);
316
                RawMagnet1a = ADC_GetValue(ADC0);
325
                RawMagnet1a = ADC_GetValue(ADC0);
317
                RawMagnet2a = -ADC_GetValue(ADC1);
326
                RawMagnet2a = -ADC_GetValue(ADC1);
318
                RawMagnet3a = ADC_GetValue(ADC7);
327
                RawMagnet3a = ADC_GetValue(ADC7);
Line 326... Line 335...
326
                RawMagnet3b = ADC_GetValue(ADC7);
335
                RawMagnet3b = ADC_GetValue(ADC7);
327
                Delay_ms(1);
336
                Delay_ms(1);
328
 
337
 
329
                CalcFields();
338
                CalcFields();
330
 
339
 
331
                //if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
340
                if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
332
                if(ExternData.CalState) Calibrate();
-
 
333
                else CalcHeading();
341
                else CalcHeading();
334
 
342
 
335
                // check data from USART
343
                // check data from USART
336
        USART0_ProcessRxData();
344
        USART0_ProcessRxData();
337
 
345