Subversion Repositories MK3Mag

Rev

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

Rev 22 Rev 23
Line 71... Line 71...
71
int16_t RawMagnet2a, RawMagnet2b;
71
int16_t RawMagnet2a, RawMagnet2b;
72
int16_t RawMagnet3a, RawMagnet3b;
72
int16_t RawMagnet3a, RawMagnet3b;
73
 
73
 
74
uint16_t Led_Timer = 0;
74
uint16_t Led_Timer = 0;
75
 
75
 
76
typedef struct
76
struct Scaling_t
77
{
77
{
78
        int16_t Range;
78
        int16_t Range;
79
        int16_t Offset;
79
        int16_t Offset;
80
} Scaling_t;
80
} ;
81
 
81
 
82
typedef struct
82
struct Calibration_t
83
{
83
{
84
        Scaling_t X;
84
        struct Scaling_t X;
85
        Scaling_t Y;
85
        struct Scaling_t Y;
86
        Scaling_t Z;
86
        struct Scaling_t Z;
87
} Calibration_t;
87
} ;
88
 
88
 
89
Calibration_t eeCalibration EEMEM; // calibration data in EEProm
89
struct Calibration_t eeCalibration EEMEM; // calibration data in EEProm
90
Calibration_t Calibration;         // calibration data in RAM
90
struct 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
 
Line 123... Line 123...
123
        {
123
        {
124
                LED_GRN_TOGGLE;
124
                LED_GRN_TOGGLE;
125
                Led_Timer = SetDelay(500);
125
                Led_Timer = SetDelay(500);
126
        }
126
        }
127
 
127
 
128
 
-
 
129
        Cx = MagnetX;
128
        Cx = MagnetX;
130
        Cy = MagnetY;
129
        Cy = MagnetY;
131
        Cz = MagnetZ;
130
        Cz = MagnetZ;
132
 
131
 
133
        if(ExternData.Orientation == 1)
132
        if(ExternData.Orientation == 1)
Line 165... Line 164...
165
 
164
 
166
        // check both sources of communication for calibration request
165
        // check both sources of communication for calibration request
167
        if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;
166
        if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;
168
        else                     cal = ExternData.CalState;
167
        else                     cal = ExternData.CalState;
169
 
168
 
-
 
169
 
170
        DebugOut.Analog[30] = cal;
170
        if(cal > 5) cal = 0;
171
        // blink code for current calibration state
171
        // blink code for current calibration state
172
        if(cal)
172
        if(cal)
173
        {
173
        {
174
                if(CheckDelay(Led_Timer) || (cal != calold))
174
                if(CheckDelay(Led_Timer) || (cal != calold))
175
                {
175
                {
Line 192... Line 192...
192
        else
192
        else
193
        {
193
        {
194
                LED_GRN_OFF;
194
                LED_GRN_OFF;
195
        }
195
        }
196
 
196
 
197
 
-
 
198
        // calibration state machine
197
        // calibration state machine
199
        switch(cal)
198
        switch(cal)
200
        {
199
        {
201
                case 0: // no calibration
200
                case 0: // no calibration
202
                        break;
201
                        break;
Line 242... Line 241...
242
                                Calibration.Z.Offset = (Zmin + Zmax) / 2;
241
                                Calibration.Z.Offset = (Zmin + Zmax) / 2;
243
                                if((Calibration.X.Range > 150) && (Calibration.Y.Range > 150) && (Calibration.Z.Range > 150))
242
                                if((Calibration.X.Range > 150) && (Calibration.Y.Range > 150) && (Calibration.Z.Range > 150))
244
                                {
243
                                {
245
                                        // indicate write process by setting the led
244
                                        // indicate write process by setting the led
246
                                        LED_GRN_ON;
245
                                        LED_GRN_ON;
247
                                        eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration_t));
246
                                        eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration));
248
                                        Delay_ms(2000);
247
                                        Delay_ms(2000);
249
                                        // reset led state
248
                                        // reset led state
250
                                        LED_GRN_OFF;
249
                                        LED_GRN_OFF;
251
                                        // reset  blinkcode
250
                                        // reset  blinkcode
252
                                        blinkcount = 0;
251
                                        blinkcount = 0;
Line 280... Line 279...
280
        DebugOut.Analog[13] = Calibration.Z.Range;
279
        DebugOut.Analog[13] = Calibration.Z.Range;
281
        DebugOut.Analog[14] = ExternData.CalState;
280
        DebugOut.Analog[14] = ExternData.CalState;
282
        DebugOut.Analog[15] = Heading;
281
        DebugOut.Analog[15] = Heading;
283
        DebugOut.Analog[16] = ExternData.UserParam[0];
282
        DebugOut.Analog[16] = ExternData.UserParam[0];
284
        DebugOut.Analog[17] = ExternData.UserParam[1];
283
        DebugOut.Analog[17] = ExternData.UserParam[1];
285
        DebugOut.Analog[30] = I2C_WriteCal.CalByte;
-
 
286
        DebugOut.Analog[31] = PC_Connected;
-
 
287
}
284
}
288
 
285
 
289
 
286
 
290
int main (void)
287
int main (void)
291
{
288
{
Line 305... Line 302...
305
 
302
 
306
    Debug_Timer = SetDelay(200);
303
    Debug_Timer = SetDelay(200);
307
    Led_Timer = SetDelay(200);
304
    Led_Timer = SetDelay(200);
308
 
305
 
309
        // read calibration info from eeprom
306
        // read calibration info from eeprom
310
        eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration_t));
307
        eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration));
311
 
308
 
312
    ExternData.Orientation = 0;
309
    ExternData.Orientation = 0;
313
    ExternData.CalState = 0;
310
    ExternData.CalState = 0;
314
    I2C_WriteCal.CalByte = 0;
311
    I2C_WriteCal.CalByte = 0;
315
 
312
 
316
        Heading = 101;
-
 
317
 
313
 
318
        // main loop
314
        // main loop
319
    while (1)
315
    while (1)
320
    {
316
    {
321
 
-
 
322
 
-
 
323
                FLIP_LOW;
317
                FLIP_LOW;
324
                Delay_ms(2);
318
                Delay_ms(2);
325
                RawMagnet1a = ADC_GetValue(ADC0);
319
                RawMagnet1a = ADC_GetValue(ADC0);
326
                RawMagnet2a = -ADC_GetValue(ADC1);
320
                RawMagnet2a = -ADC_GetValue(ADC1);
327
                RawMagnet3a = ADC_GetValue(ADC7);
321
                RawMagnet3a = ADC_GetValue(ADC7);
Line 340... Line 334...
340
                if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
334
                if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
341
                else CalcHeading();
335
                else CalcHeading();
342
 
336
 
343
                // check data from USART
337
                // check data from USART
344
        USART0_ProcessRxData();
338
        USART0_ProcessRxData();
-
 
339
                USART0_TransmitTxData();
345
 
340
 
346
        if(PC_Connected)
341
        if(PC_Connected)
347
        {
342
        {
348
            USART0_EnableTXD();
343
            USART0_EnableTXD();
349
            USART0_TransmitTxData();
344
            USART0_TransmitTxData();