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; |
92,7 → 95,7 |
|
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,18 → 220,18 |
|
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 |
// Save values |
if(cal != calold) // avoid continously wrinting of eeprom! |
{ |
Calibration.X.Range = Xmax - Xmin; |
Calibration.X.Offset = (Xmin + Xmax) / 2; |
Calibration.Y.Range = Ymax - Ymin; |
202,16 → 240,23 |
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 |