67,9 → 67,7 |
#include "uart.h" |
|
|
int16_t RawMagnet1a, RawMagnet1b; // raw AD-Data |
int16_t RawMagnet2a, RawMagnet2b; |
int16_t RawMagnet3a, RawMagnet3b; |
AttitudeSource_t AttitudeSource = ATTITUDE_SOURCE_ACC; |
|
uint16_t Led_Timer = 0; |
|
81,18 → 79,27 |
|
struct Calibration_t |
{ |
struct Scaling_t X; |
struct Scaling_t Y; |
struct Scaling_t Z; |
struct Scaling_t MagX; |
struct Scaling_t MagY; |
struct Scaling_t MagZ; |
struct Scaling_t AccX; |
struct Scaling_t AccY; |
struct Scaling_t AccZ; |
} ; |
|
struct Calibration_t eeCalibration EEMEM; // calibration data in EEProm |
struct Calibration_t Calibration; // calibration data in RAM |
struct Calibration_t eeCalibration EEMEM; // calibration data in EEProm |
struct 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 |
|
// magnet sensor variable |
int16_t RawMagnet1a, RawMagnet1b; // raw magnet sensor data |
int16_t RawMagnet2a, RawMagnet2b; |
int16_t RawMagnet3a, RawMagnet3b; |
int16_t UncalMagX, UncalMagY, UncalMagZ; // sensor signal difference without Scaling |
int16_t MagX, MagY, MagZ; // rescaled magnetic field readings |
// acc sensor variables |
int16_t RawAccX, RawAccY, RawAccZ; // raw acceleration readings |
int16_t AccX, AccY, AccZ; // rescaled acceleration readings |
uint8_t AccPresent = 0; |
uint8_t PC_Connected = 0; |
|
int16_t Heading = -1; |
100,16 → 107,16 |
|
void CalcFields(void) |
{ |
UncalMagnetX = (RawMagnet1a - RawMagnet1b) / 2; |
UncalMagnetY = (RawMagnet3a - RawMagnet3b) / 2; |
UncalMagnetZ = (RawMagnet2a - RawMagnet2b) / 2; |
UncalMagX = (RawMagnet1a - RawMagnet1b); |
UncalMagY = (RawMagnet3a - RawMagnet3b); |
UncalMagZ = (RawMagnet2a - RawMagnet2b); |
|
if(Calibration.X.Range != 0) MagnetX = (1024L * (int32_t)(UncalMagnetX - Calibration.X.Offset)) / (Calibration.X.Range); |
else MagnetX = 0; |
if(Calibration.Y.Range != 0) MagnetY = (1024L * (int32_t)(UncalMagnetY - Calibration.Y.Offset)) / (Calibration.Y.Range); |
else MagnetY = 0; |
if(Calibration.Y.Range != 0) MagnetZ = (1024L * (int32_t)(UncalMagnetZ - Calibration.Z.Offset)) / (Calibration.Z.Range); |
else MagnetZ = 0; |
if(Calibration.MagX.Range != 0) MagX = (1024L * (int32_t)(UncalMagX - Calibration.MagX.Offset)) / (Calibration.MagX.Range); |
else MagX = 0; |
if(Calibration.MagY.Range != 0) MagY = (1024L * (int32_t)(UncalMagY - Calibration.MagY.Offset)) / (Calibration.MagY.Range); |
else MagY = 0; |
if(Calibration.MagY.Range != 0) MagZ = (1024L * (int32_t)(UncalMagZ - Calibration.MagZ.Offset)) / (Calibration.MagZ.Range); |
else MagZ = 0; |
} |
|
|
125,20 → 132,49 |
Led_Timer = SetDelay(500); |
} |
|
Cx = MagnetX; |
Cy = MagnetY; |
Cz = MagnetZ; |
Cx = MagX; |
Cy = MagY; |
Cz = MagZ; |
|
if(ExternData.Orientation == 1) |
{ |
Cx = MagnetX; |
Cy = -MagnetY; |
Cz = MagnetZ; |
Cx = MagX; |
Cy = -MagY; |
Cz = MagZ; |
} |
|
// calculate nick and roll angle in rad |
nick_rad = ((double)I2C_WriteAttitude.Nick) * M_PI / (double)(1800.0); |
roll_rad = ((double)I2C_WriteAttitude.Roll) * M_PI / (double)(1800.0); |
switch(AttitudeSource) |
{ |
case ATTITUDE_SOURCE_I2C: |
nick_rad = ((double)I2C_WriteAttitude.Nick) * M_PI / (double)(1800.0); |
roll_rad = ((double)I2C_WriteAttitude.Roll) * M_PI / (double)(1800.0); |
break; |
|
case ATTITUDE_SOURCE_UART: |
nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800.0); |
roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800.0); |
break; |
|
case ATTITUDE_SOURCE_ACC: |
if(AccX > 125) nick_rad = M_PI / 2; |
else |
if(AccX < -125) nick_rad = -M_PI / 2; |
else |
{ |
nick_rad = asin((double) AccX / 125.0); |
} |
|
if(AccY > 125) roll_rad = M_PI / 2; |
else |
if(AccY < -125) roll_rad = -M_PI / 2; |
else |
{ |
roll_rad = asin((double) AccY / 125.0); |
} |
break; |
} |
|
// calculate attitude correction |
Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad); |
Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad); |
206,14 → 242,17 |
Ymax = -10000; |
Zmin = 10000; |
Zmax = -10000; |
Calibration.AccX.Offset = RawAccX; |
Calibration.AccY.Offset = RawAccY; |
Calibration.AccZ.Offset = RawAccZ; |
break; |
|
case 2: // 2nd step of calibration |
// find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane |
if(UncalMagnetX < Xmin) Xmin = UncalMagnetX; |
if(UncalMagnetX > Xmax) Xmax = UncalMagnetX; |
if(UncalMagnetY < Ymin) Ymin = UncalMagnetY; |
if(UncalMagnetY > Ymax) Ymax = UncalMagnetY; |
if(UncalMagX < Xmin) Xmin = UncalMagX; |
if(UncalMagX > Xmax) Xmax = UncalMagX; |
if(UncalMagY < Ymin) Ymin = UncalMagY; |
if(UncalMagY > Ymax) Ymax = UncalMagY; |
break; |
|
case 3: // 3rd step of calibration |
222,8 → 261,8 |
|
case 4: |
// find Min and Max of the Z-Sensor |
if(UncalMagnetZ < Zmin) Zmin = UncalMagnetZ; |
if(UncalMagnetZ > Zmax) Zmax = UncalMagnetZ; |
if(UncalMagZ < Zmin) Zmin = UncalMagZ; |
if(UncalMagZ > Zmax) Zmax = UncalMagZ; |
break; |
|
case 5: |
230,13 → 269,13 |
// Save values |
if(cal != calold) // avoid continously writing of eeprom! |
{ |
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)) |
Calibration.MagY.Range = Xmax - Xmin; |
Calibration.MagX.Offset = (Xmin + Xmax) / 2; |
Calibration.MagY.Range = Ymax - Ymin; |
Calibration.MagY.Offset = (Ymin + Ymax) / 2; |
Calibration.MagZ.Range = Zmax - Zmin; |
Calibration.MagZ.Offset = (Zmin + Zmax) / 2; |
if((Calibration.MagX.Range > 150) && (Calibration.MagY.Range > 150) && (Calibration.MagZ.Range > 150)) |
{ |
// indicate write process by setting the led |
LED_GRN_ON; |
260,27 → 299,71 |
|
void SetDebugValues(void) |
{ |
DebugOut.Analog[0] = MagnetX; |
DebugOut.Analog[1] = MagnetY; |
DebugOut.Analog[2] = MagnetZ; |
DebugOut.Analog[3] = UncalMagnetX; |
DebugOut.Analog[4] = UncalMagnetY; |
DebugOut.Analog[5] = UncalMagnetZ; |
DebugOut.Analog[6] = I2C_WriteAttitude.Nick; |
DebugOut.Analog[7] = I2C_WriteAttitude.Roll; |
DebugOut.Analog[8] = Calibration.X.Offset; |
DebugOut.Analog[9] = Calibration.X.Range; |
DebugOut.Analog[10] = Calibration.Y.Offset; |
DebugOut.Analog[11] = Calibration.Y.Range; |
DebugOut.Analog[12] = Calibration.Z.Offset; |
DebugOut.Analog[13] = Calibration.Z.Range; |
DebugOut.Analog[0] = MagX; |
DebugOut.Analog[1] = MagY; |
DebugOut.Analog[2] = MagZ; |
DebugOut.Analog[3] = UncalMagX; |
DebugOut.Analog[4] = UncalMagY; |
DebugOut.Analog[5] = UncalMagZ; |
switch(AttitudeSource) |
{ |
case ATTITUDE_SOURCE_ACC: |
|
break; |
|
case ATTITUDE_SOURCE_UART: |
DebugOut.Analog[6] = ExternData.Attitude[NICK]; |
DebugOut.Analog[7] = ExternData.Attitude[ROLL]; |
break; |
|
|
case ATTITUDE_SOURCE_I2C: |
DebugOut.Analog[6] = I2C_WriteAttitude.Nick; |
DebugOut.Analog[7] = I2C_WriteAttitude.Roll; |
break; |
} |
DebugOut.Analog[8] = Calibration.MagX.Offset; |
DebugOut.Analog[9] = Calibration.MagX.Range; |
DebugOut.Analog[10] = Calibration.MagY.Offset; |
DebugOut.Analog[11] = Calibration.MagY.Range; |
DebugOut.Analog[12] = Calibration.MagZ.Offset; |
DebugOut.Analog[13] = Calibration.MagZ.Range; |
DebugOut.Analog[14] = ExternData.CalState; |
DebugOut.Analog[15] = Heading; |
DebugOut.Analog[16] = ExternData.UserParam[0]; |
DebugOut.Analog[17] = ExternData.UserParam[1]; |
DebugOut.Analog[18] = AccX; |
DebugOut.Analog[19] = AccY; |
DebugOut.Analog[20] = AccZ; |
DebugOut.Analog[21] = RawAccX; |
DebugOut.Analog[22] = RawAccY; |
DebugOut.Analog[23] = RawAccZ; |
DebugOut.Analog[24] = Calibration.AccX.Offset; |
DebugOut.Analog[25] = Calibration.AccY.Offset; |
DebugOut.Analog[26] = Calibration.AccZ.Offset; |
} |
|
void AccMeasurement(void) |
{ |
if(AccPresent) |
{ |
RawAccX = (RawAccX + (int16_t)ADC_GetValue(ADC2))/2; |
RawAccY = (RawAccY + (int16_t)ADC_GetValue(ADC3))/2; |
RawAccZ = (RawAccZ + (int16_t)ADC_GetValue(ADC6))/2; |
} |
else |
{ |
RawAccX = 0; |
RawAccY = 0; |
RawAccZ = 0; |
} |
AccX = ((RawAccX - Calibration.AccX.Offset) + AccX * 7) / 8; |
AccY = ((RawAccY - Calibration.AccY.Offset) + AccY * 7) / 8; |
AccZ = ((Calibration.AccZ.Offset - RawAccZ) + AccZ * 7) / 8; |
} |
|
|
|
int main (void) |
{ |
// reset input pullup |
316,14 → 399,15 |
RawMagnet1a = ADC_GetValue(ADC0); |
RawMagnet2a = -ADC_GetValue(ADC1); |
RawMagnet3a = ADC_GetValue(ADC7); |
AccMeasurement(); |
Delay_ms(1); |
|
|
FLIP_HIGH; |
Delay_ms(2); |
RawMagnet1b = ADC_GetValue(ADC0); |
RawMagnet2b = -ADC_GetValue(ADC1); |
RawMagnet3b = ADC_GetValue(ADC7); |
AccMeasurement(); |
Delay_ms(1); |
|
CalcFields(); |