| 57,6 → 57,7 |
|---|
| #include <avr/interrupt.h> |
| #include <math.h> |
| #include <stdlib.h> |
| #include <stdio.h> |
| |
| #include "main.h" |
| #include "timer0.h" |
| 70,6 → 71,8 |
|---|
| int16_t RawMagnet2a, RawMagnet2b; |
| int16_t RawMagnet3a, RawMagnet3b; |
| |
| uint16_t Led_Timer = 0; |
| |
| typedef struct |
| { |
| int16_t Range; |
| 87,12 → 90,12 |
|---|
| Calibration_t Calibration; // calibration data in RAM |
| |
| |
| int16_t UncalMagnetX, UncalMagnetY, UncalMagnetZ; // sensor signal difference without Scaling |
| int16_t MagnetX, MagnetY, MagnetZ; // rescaled magnetic field readings |
| int16_t UncalMagnetX, UncalMagnetY, UncalMagnetZ; // sensor signal difference without Scaling |
| int16_t MagnetX, MagnetY, MagnetZ; // rescaled magnetic field readings |
| |
| uint8_t PC_Connected = 0; |
| |
| int16_t Heading; |
| int16_t Heading = -1; |
| |
| |
| void CalcFields(void) |
| 112,7 → 115,14 |
|---|
| double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz; |
| int16_t heading = -1; |
| |
| // blink code for normal operation |
| if(CheckDelay(Led_Timer)) |
| { |
| LED_GRN_TOGGLE; |
| Led_Timer = SetDelay(500); |
| } |
| |
| |
| Cx = MagnetX; |
| Cy = MagnetY; |
| Cz = MagnetZ; |
| 146,23 → 156,52 |
|---|
| void Calibrate(void) |
| { |
| uint8_t cal; |
| static uint8_t calold = 0; |
| static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0; |
| |
| static uint8_t blinkcount = 0; |
| /* |
| // check both sources of communication for calibration request |
| if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte; |
| else cal = ExternData.CalState; |
| else cal = ExternData.CalState;*/ |
| |
| cal = ExternData.CalState; |
| |
| // blink code for current calibration state |
| if(cal) |
| { |
| if(CheckDelay(Led_Timer) || (cal != calold)) |
| { |
| if(blinkcount & 0x01) LED_GRN_OFF; |
| else LED_GRN_ON; |
| |
| // end of blinkcount sequence |
| if( (blinkcount + 1 ) >= (2 * cal) ) |
| { |
| blinkcount = 0; |
| Led_Timer = SetDelay(1000); |
| } |
| else |
| { |
| blinkcount++; |
| Led_Timer = SetDelay(170); |
| } |
| } |
| } |
| else |
| { |
| LED_GRN_OFF; |
| } |
| |
| |
| // calibration state machine |
| switch(cal) |
| { |
| case 0: // no calibration |
| LED_GRN_ON; |
| break; |
| |
| case 1: // 1st step of calibration |
| // initialize ranges |
| // used to change the orientation of the MK3MAG in the horizontal plane |
| LED_GRN_OFF; |
| Xmin = 10000; |
| Xmax = -10000; |
| Ymin = 10000; |
| 173,7 → 212,6 |
|---|
| |
| case 2: // 2nd step of calibration |
| // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane |
| LED_GRN_ON; |
| if(UncalMagnetX < Xmin) Xmin = UncalMagnetX; |
| if(UncalMagnetX > Xmax) Xmax = UncalMagnetX; |
| if(UncalMagnetY < Ymin) Ymin = UncalMagnetY; |
| 182,36 → 220,43 |
|---|
| |
| case 3: // 3rd step of calibration |
| // used to change the orietation of the MK3MAG vertical to the horizontal plane |
| LED_GRN_OFF; |
| break; |
| |
| case 4: |
| // find Min and Max of the Z-Sensor |
| LED_GRN_ON; |
| if(UncalMagnetZ < Zmin) Zmin = UncalMagnetZ; |
| if(UncalMagnetZ > Zmax) Zmax = UncalMagnetZ; |
| break; |
| |
| case 5: |
| LED_GRN_OFF; // Save values |
| Calibration.X.Range = Xmax - Xmin; |
| Calibration.X.Offset = (Xmin + Xmax) / 2; |
| Calibration.Y.Range = Ymax - Ymin; |
| Calibration.Y.Offset = (Ymin + Ymax) / 2; |
| Calibration.Z.Range = Zmax - Zmin; |
| Calibration.Z.Offset = (Zmin + Zmax) / 2; |
| if((Calibration.X.Range > 150) && (Calibration.Y.Range > 150) && (Calibration.Z.Range > 150)) |
| // Save values |
| if(cal != calold) // avoid continously wrinting of eeprom! |
| { |
| eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration_t)); |
| Delay_ms(2000); |
| Calibration.X.Range = Xmax - Xmin; |
| Calibration.X.Offset = (Xmin + Xmax) / 2; |
| Calibration.Y.Range = Ymax - Ymin; |
| Calibration.Y.Offset = (Ymin + Ymax) / 2; |
| Calibration.Z.Range = Zmax - Zmin; |
| Calibration.Z.Offset = (Zmin + Zmax) / 2; |
| if((Calibration.X.Range > 150) && (Calibration.Y.Range > 150) && (Calibration.Z.Range > 150)) |
| { |
| // indicate write process by setting the led |
| LED_GRN_ON; |
| eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration_t)); |
| Delay_ms(2000); |
| // reset led state |
| LED_GRN_OFF; |
| // reset blinkcode |
| blinkcount = 0; |
| Led_Timer = SetDelay(1000); |
| } |
| } |
| LED_GRN_ON; |
| break; |
| |
| default: |
| LED_GRN_ON; |
| break; |
| } |
| calold = cal; |
| } |
| |
| |
| 235,6 → 280,7 |
|---|
| DebugOut.Analog[15] = Heading; |
| DebugOut.Analog[16] = ExternData.UserParam[0]; |
| DebugOut.Analog[17] = ExternData.UserParam[1]; |
| DebugOut.Analog[31] = PC_Connected; |
| } |
| |
| |
| 247,9 → 293,13 |
|---|
| ADC_Init(); |
| I2C_Init(); |
| |
| |
| sei(); //Globale Interrupts Einschalten |
| |
| |
| |
| Debug_Timer = SetDelay(100); // Sendeintervall |
| Led_Timer = SetDelay(100); |
| |
| // read calibration info from eeprom |
| eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration_t)); |
| 268,6 → 318,7 |
|---|
| RawMagnet3a = ADC_GetValue(ADC7); |
| Delay_ms(1); |
| |
| |
| FLIP_HIGH; |
| Delay_ms(2); |
| RawMagnet1b = ADC_GetValue(ADC0); |
| 277,7 → 328,8 |
|---|
| |
| CalcFields(); |
| |
| if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate(); |
| //if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate(); |
| if(ExternData.CalState) Calibrate(); |
| else CalcHeading(); |
| |
| // check data from USART |