Subversion Repositories MK3Mag

Compare Revisions

Ignore whitespace Rev 27 → Rev 28

/branches/MK3Mag V0.14 Code Redesign Killagreg/main.c
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();
/branches/MK3Mag V0.14 Code Redesign Killagreg/main.h
9,12 → 9,19
 
#define SYSCLK 8000000L //Quarz Frequenz in Hz
 
typedef enum
{
ATTITUDE_SOURCE_I2C,
ATTITUDE_SOURCE_UART,
ATTITUDE_SOURCE_ACC
} AttitudeSource_t;
 
 
extern uint8_t PC_Connected;
 
extern int16_t Heading;
extern AttitudeSource_t AttitudeSource;
 
extern int16_t MagnetX, MagnetY, MagnetZ;
extern int16_t MagX, MagY, MagZ;
 
void SetDebugValues(void);
 
/branches/MK3Mag V0.14 Code Redesign Killagreg/twislave.c
175,9 → 175,9
I2C_RxBuffer = 0;
I2C_RxBufferSize = 0;
 
I2C_Mag.MagX = MagnetX;
I2C_Mag.MagY = MagnetY;
I2C_Mag.MagZ = MagnetZ;
I2C_Mag.MagX = MagX;
I2C_Mag.MagY = MagY;
I2C_Mag.MagZ = MagZ;
break;
 
case I2C_CMD_READ_HEADING:
186,6 → 186,7
I2C_RxBuffer = (uint8_t *)&I2C_WriteAttitude;
I2C_RxBufferSize = sizeof(I2C_WriteAttitude);
I2C_Heading.Heading = Heading;
AttitudeSource = ATTITUDE_SOURCE_I2C;
break;
default: // unknown command id
I2C_RxBuffer = 0;
/branches/MK3Mag V0.14 Code Redesign Killagreg/uart.c
100,30 → 100,30
"Magnet X ", //0
"Magnet Y ",
"Magnet Z ",
"Raw X ",
"Raw Y ",
"Raw Z ", //5
"RawMagnet X ",
"RawMagnet Y ",
"RawMagnet Z ", //5
"Attitude Nick ",
"Attitude Roll ",
"XOffset ",
"XRange ",
"YOffset ", //10
"YRange ",
"ZOffset ",
"ZRange ",
"Magnet X Offset ",
"Magnet X Range ",
"Magnet Y Offset ", //10
"Magnet Y Range ",
"Magnet Z Offset ",
"Magnet Z Range ",
"Calstate ",
"Heading ", //15
"User0 ",
"User1 ",
"Analog18 ",
"Analog19 ",
"Analog20 ", //20
"Analog21 ",
"Analog22 ",
"Analog23 ",
"Analog24 ",
"Analog25 ", //25
"Analog26 ",
"Acc X ",
"Acc Y ",
"Acc Z ", //20
"RawAcc X ",
"RawAcc Y ",
"RawAcc Z ",
"Acc X Offset ",
"Acc Y Offset ", //25
"Acc Z Offset ",
"Analog27 ",
"Analog28 ",
"Analog29 ",
420,6 → 420,7
Decode64((uint8_t *) &ExternData, sizeof(ExternData), 3, ReceivedBytes);
I2C_WriteAttitude.Nick = ExternData.Attitude[NICK];
I2C_WriteAttitude.Nick = ExternData.Attitude[ROLL];
AttitudeSource = ATTITUDE_SOURCE_UART;
RequestFlags |= COMPASS_HEADING;
break;