Subversion Repositories MK3Mag

Compare Revisions

Ignore whitespace Rev 28 → Rev 29

/branches/MK3Mag V0.14 Code Redesign Killagreg/main.c
68,6 → 68,7
 
 
AttitudeSource_t AttitudeSource = ATTITUDE_SOURCE_ACC;
Orientation_t Orientation = ORIENTATION_FC;
 
uint16_t Led_Timer = 0;
 
85,7 → 86,7
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
95,16 → 96,16
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 MagX = 0, MagY = 0, MagZ = 0; // rescaled magnetic field readings
 
int16_t Heading = -1;
// acceleration sensor variables
int16_t RawAccX = 0, RawAccY = 0, RawAccZ = 0; // raw acceleration readings
int16_t AccX = 0, AccY = 0, AccZ = 0; // rescaled acceleration readings
int16_t AccAttitudeNick = 0, AccAttitudeRoll = 0; // nick and roll angle from acc
 
int16_t Heading = -1; // the current compass heading in deg
 
 
void CalcFields(void)
{
UncalMagX = (RawMagnet1a - RawMagnet1b);
117,12 → 118,46
else MagY = 0;
if(Calibration.MagY.Range != 0) MagZ = (1024L * (int32_t)(UncalMagZ - Calibration.MagZ.Offset)) / (Calibration.MagZ.Range);
else MagZ = 0;
 
if(AccPresent)
{
AccX = (RawAccX - Calibration.AccX.Offset);
AccY = (RawAccY - Calibration.AccY.Offset);
AccZ = (Calibration.AccZ.Offset - RawAccZ);
 
#if (BOARD == 10) // the hardware 1.0 has the LIS3L02AL
// acc mode assumes orientation like FC
if(AccX > 136) AccAttitudeNick = -800;
else
if(AccX < -136) AccAttitudeNick = 800;
else AccAttitudeNick = (int16_t)(-1800.0 * asin((double) AccX / 138.0) / M_PI);
 
 
if(AccY > 136) AccAttitudeRoll = 800;
else
if(AccY < -136) AccAttitudeRoll = -800;
else AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI);
 
#else // the hardware 1.1 has the LIS344ALH with a different axis definition (X -> -Y, Y -> X, Z -> Z)
// acc mode assumes orientation like FC
if(AccY > 136) AccAttitudeNick = 800;
else
if(AccY < -136) AccAttitudeNick = -800;
else AccAttitudeNick = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI);
 
 
if(AccX > 136) AccAttitudeRoll = 800;
else
if(AccX < -136) AccAttitudeRoll = -800;
else AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccX / 138.0) / M_PI);
#endif
}
}
 
 
void CalcHeading(void)
{
double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz;
double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0;
int16_t heading = -1;
 
// blink code for normal operation
132,15 → 167,20
Led_Timer = SetDelay(500);
}
 
Cx = MagX;
Cy = MagY;
Cz = MagZ;
switch(Orientation)
{
case ORIENTATION_NC:
Cx = MagX;
Cy = MagY;
Cz = MagZ;
break;
 
if(ExternData.Orientation == 1)
{
Cx = MagX;
Cy = -MagY;
Cz = MagZ;
case ORIENTATION_FC:
// rotation of 90 deg compared to NC setup
Cx = MagY;
Cy = -MagX;
Cz = MagZ;
break;
}
 
// calculate nick and roll angle in rad
157,22 → 197,14
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);
}
nick_rad = ((double)AccAttitudeNick) * M_PI / (double)(1800.0);
roll_rad = ((double)AccAttitudeRoll) * M_PI / (double)(1800.0);
break;
 
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;
default:
nick_rad = 0;
roll_rad = 0;
break;
}
 
// calculate attitude correction
179,6 → 211,10
Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad);
Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad);
 
DebugOut.Analog[27] = (int16_t)Hx;
DebugOut.Analog[28] = (int16_t)Hy;
 
 
// calculate Heading
heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI);
// atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation
269,7 → 305,7
// Save values
if(cal != calold) // avoid continously writing of eeprom!
{
Calibration.MagY.Range = Xmax - Xmin;
Calibration.MagX.Range = Xmax - Xmin;
Calibration.MagX.Offset = (Xmin + Xmax) / 2;
Calibration.MagY.Range = Ymax - Ymin;
Calibration.MagY.Offset = (Ymin + Ymax) / 2;
280,12 → 316,9
// indicate write process by setting the led
LED_GRN_ON;
eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration));
Delay_ms(2000);
// reset led state
LED_GRN_OFF;
Led_Timer = SetDelay(2000);
// reset blinkcode
blinkcount = 0;
Led_Timer = SetDelay(1000);
}
}
break;
308,7 → 341,8
switch(AttitudeSource)
{
case ATTITUDE_SOURCE_ACC:
 
DebugOut.Analog[6] = AccAttitudeNick;
DebugOut.Analog[7] = AccAttitudeRoll;
break;
 
case ATTITUDE_SOURCE_UART:
341,6 → 375,7
DebugOut.Analog[24] = Calibration.AccX.Offset;
DebugOut.Analog[25] = Calibration.AccY.Offset;
DebugOut.Analog[26] = Calibration.AccZ.Offset;
DebugOut.Analog[29] = AttitudeSource;
}
 
void AccMeasurement(void)
347,9 → 382,9
{
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;
RawAccX = (RawAccX + (int16_t)ADC_GetValue(ACC_X))/2;
RawAccY = (RawAccY + (int16_t)ADC_GetValue(ACC_Y))/2;
RawAccZ = (RawAccZ + (int16_t)ADC_GetValue(ACC_Z))/2;
}
else
{
357,13 → 392,8
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
378,6 → 408,11
 
sei(); // enable globale interrupts
 
if(AccPresent)
{
USART0_Print("ACC present\n");
}
 
LED_GRN_ON;
 
Debug_Timer = SetDelay(200);
386,7 → 421,6
// read calibration info from eeprom
eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration));
 
ExternData.Orientation = 0;
ExternData.CalState = 0;
I2C_WriteCal.CalByte = 0;
 
396,17 → 430,17
{
FLIP_LOW;
Delay_ms(2);
RawMagnet1a = ADC_GetValue(ADC0);
RawMagnet2a = -ADC_GetValue(ADC1);
RawMagnet3a = ADC_GetValue(ADC7);
RawMagnet1a = ADC_GetValue(MAG_X);
RawMagnet2a = -ADC_GetValue(MAG_Y);
RawMagnet3a = ADC_GetValue(MAG_Z);
AccMeasurement();
Delay_ms(1);
 
FLIP_HIGH;
Delay_ms(2);
RawMagnet1b = ADC_GetValue(ADC0);
RawMagnet2b = -ADC_GetValue(ADC1);
RawMagnet3b = ADC_GetValue(ADC7);
RawMagnet1b = ADC_GetValue(MAG_X);
RawMagnet2b = -ADC_GetValue(MAG_Y);
RawMagnet3b = ADC_GetValue(MAG_Z);
AccMeasurement();
Delay_ms(1);
 
417,8 → 451,16
 
// check data from USART
USART0_ProcessRxData();
USART0_TransmitTxData();
 
if(NC_Connected) NC_Connected--;
if(FC_Connected) FC_Connected--;
// fall back to attitude estimation from acc sensor if NC or FC does'nt send attittude data
if(!NC_Connected && ! NC_Connected)
{
AttitudeSource = ATTITUDE_SOURCE_ACC;
Orientation = ORIENTATION_FC;
}
 
if(PC_Connected)
{
USART0_EnableTXD();