Subversion Repositories MK3Mag

Compare Revisions

Ignore whitespace Rev 54 → Rev 55

/trunk/main.c
156,7 → 156,7
 
void CalcHeading(void)
{
double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0;
double nick_rad, roll_rad, Cx = 0, Cy = 0, Cz = 0, Hx = 0, Hy = 0;
int16_t nick, roll;
int16_t heading = -1;
 
167,72 → 167,73
else LED_GRN_ON;
Led_Timer = SetDelay(150);
}
switch(Orientation)
 
if(Calibration.Version != CALIBRATION_VERSION) heading = -1; // Version of the calibration Data does not match
else
{
case ORIENTATION_NC:
Cx = (double)MagX;
Cy = (double)MagY;
Cz = (double)MagZ;
break;
switch(Orientation)
{
case ORIENTATION_NC:
Cx = (double)MagX;
Cy = (double)MagY;
Cz = (double)MagZ;
break;
 
case ORIENTATION_FC:
// rotation of 90 deg compared to NC setup
Cx = (double)MagY;
Cy = -(double)MagX;
Cz = (double)MagZ;
break;
}
case ORIENTATION_FC:
// rotation of 90 deg compared to NC setup
Cx = (double)MagY;
Cy = -(double)MagX;
Cz = (double)MagZ;
break;
}
 
// calculate nick and roll angle in rad
switch(AttitudeSource)
{
case ATTITUDE_SOURCE_I2C:
cli(); // stop interrupts
nick = I2C_WriteAttitude.Nick;
roll = I2C_WriteAttitude.Roll;
sei(); // start interrupts
// calculate nick and roll angle in rad
switch(AttitudeSource)
{
case ATTITUDE_SOURCE_I2C:
cli(); // stop interrupts
nick = I2C_WriteAttitude.Nick;
roll = I2C_WriteAttitude.Roll;
sei(); // start interrupts
break;
case ATTITUDE_SOURCE_UART:
cli(); // stop interrupts
nick = ExternData.Attitude[NICK];
roll = ExternData.Attitude[ROLL];
sei(); // start interrupts
break;
case ATTITUDE_SOURCE_ACC:
nick = AccAttitudeNick;
roll = AccAttitudeRoll;
break;
default:
nick = 0;
roll = 0;
break;
case ATTITUDE_SOURCE_UART:
cli(); // stop interrupts
nick = ExternData.Attitude[NICK];
roll = ExternData.Attitude[ROLL];
sei(); // start interrupts
break;
case ATTITUDE_SOURCE_ACC:
nick = AccAttitudeNick;
roll = AccAttitudeRoll;
break;
default:
nick = 0;
roll = 0;
break;
}
}
 
nick_rad = ((double)nick * M_PI) / 1800.0;
roll_rad = ((double)roll * M_PI) / 1800.0;
nick_rad = ((double)nick * M_PI) / 1800.0;
roll_rad = ((double)roll * M_PI) / 1800.0;
 
// calculate attitude correction
Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad);
Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad);
// calculate attitude correction
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;
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
// but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
if (heading < 0) heading = -heading;
else heading = 360 - heading;
 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(Calibration.Version != CALIBRATION_VERSION) heading = -1; // Version of the calibration Data does not match
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
cli(); // stop interrupts
if(abs(heading) < 361) Heading = heading;
else (Heading = -1);
sei(); // start interrupts
 
// 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
// but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
if (heading < 0) heading = -heading;
else heading = 360 - heading;
}
// stop interrrupts during heading update to avoid transmitting heading between the update of both bytes of the int16_t
cli(); // stop interrupts
if(abs(heading) < 361) Heading = heading;
else (Heading = -1);
sei(); // start interrupts
}
 
void Calibrate(void)
333,6 → 334,11
// reset blinkcode
blinkcount = 0;
}
else
{
// restore old calibration data from eeprom
eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration));
}
}
invert_blinking = 0;
break;
411,6 → 417,8
 
int main (void)
{
static uint8_t state = 0;
static uint16_t statetimer;
// reset input pullup
DDRC &=~((1<<DDC6));
PORTC |= (1<<PORTC6);
438,48 → 446,82
ExternData.CalState = 0;
I2C_WriteCal.CalByte = 0;
 
statetimer = SetDelay(0);
 
// main loop
while (1)
{
FLIP_LOW;
Delay_ms(2);
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(MAG_X);
RawMagnet2b = -ADC_GetValue(MAG_Y);
RawMagnet3b = ADC_GetValue(MAG_Z);
AccMeasurement();
Delay_ms(1);
switch(state)
{
case 0:
if(CheckDelay(statetimer))
{
FLIP_LOW;
statetimer = SetDelay(2);
state = 1;
}
break;
 
CalcFields();
case 1:
if(CheckDelay(statetimer))
{
RawMagnet1a = ADC_GetValue(MAG_X);
RawMagnet2a = -ADC_GetValue(MAG_Y);
RawMagnet3a = ADC_GetValue(MAG_Z);
AccMeasurement();
statetimer = SetDelay(1);
state = 2;
}
break;
 
if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
else CalcHeading();
case 2:
if(CheckDelay(statetimer))
{
FLIP_HIGH;
statetimer = SetDelay(2);
state = 3;
}
break;
 
case 3:
if(CheckDelay(statetimer))
{
RawMagnet1b = ADC_GetValue(MAG_X);
RawMagnet2b = -ADC_GetValue(MAG_Y);
RawMagnet3b = ADC_GetValue(MAG_Z);
AccMeasurement();
statetimer = SetDelay(1);
state = 0;
 
CalcFields();
if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
else CalcHeading();
if(NC_Connected) NC_Connected--;
if(FC_Connected) FC_Connected--;
if(PC_Connected) PC_Connected--;
// fall back to attitude estimation from onboard acc sensor if NC or FC does'nt send attitude data
if(!FC_Connected && ! NC_Connected)
{
AttitudeSource = ATTITUDE_SOURCE_ACC;
Orientation = ORIENTATION_FC;
}
}
break;
 
default: // should never happen
statetimer = SetDelay(0);
state = 0;
break;
} // one full state cylce is 6 ms
 
// check data from USART
USART0_ProcessRxData();
 
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(!FC_Connected && ! NC_Connected)
{
AttitudeSource = ATTITUDE_SOURCE_ACC;
Orientation = ORIENTATION_FC;
}
 
if(PC_Connected)
{
USART0_EnableTXD();
USART0_TransmitTxData();
PC_Connected--;
}
else
{
/trunk/uart.c
145,9 → 145,9
"Acc X Offset ",
"Acc Y Offset ", //25
"Acc Z Offset ",
"Analog27 ",
"Analog28 ",
"Analog29 ",
"Heading X ",
"Heading Y ",
"Attitude Source ",
"I2C Error ", //30
"I2C Okay "
};
255,7 → 255,8
DDRD |= (1<<DDD1); // set TXD pin as output
PORTD &= ~(1 << PORTD1);
UCSR0B |= (1 << TXEN0); // enable TX in USART
UCSR0B |= (1 << TXCIE0); // disable TX-Interrupt
//changing the interrupt flag yields to strange effects
//UCSR0B |= (1 << TXCIE0); // enable TX-Interrupt
}
 
// ---------------------------------------------------------------------------------
262,9 → 263,9
void USART0_DisableTXD(void)
{
while(!txd_complete){ };
 
UCSR0B &= ~(1 << TXCIE0); // disable TX-Interrupt
UCSR0B &= ~(1 << TXEN0); // disable TXD in USART
//changing the interrupt flag yields to strange effects
//UCSR0B &= ~(1 << TXCIE0); // disable TX-Interrupt
UCSR0B &= ~(1 << TXEN0); // disable TX in USART
DDRD &= ~(1<<DDD1); // set TXD pin as input
PORTD &= ~(1 << PORTD1);
}