Subversion Repositories MK3Mag

Compare Revisions

Ignore whitespace Rev 52 → Rev 53

/trunk/main.c
113,7 → 113,6
UncalMagX = (RawMagnet1a - RawMagnet1b);
UncalMagY = (RawMagnet3a - RawMagnet3b);
UncalMagZ = (RawMagnet2a - RawMagnet2b);
 
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);
120,7 → 119,6
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);
172,16 → 170,16
switch(Orientation)
{
case ORIENTATION_NC:
Cx = MagX;
Cy = MagY;
Cz = MagZ;
Cx = (double)MagX;
Cy = (double)MagY;
Cz = (double)MagZ;
break;
 
case ORIENTATION_FC:
// rotation of 90 deg compared to NC setup
Cx = MagY;
Cy = -MagX;
Cz = MagZ;
Cx = (double)MagY;
Cy = -(double)MagX;
Cz = (double)MagZ;
break;
}
 
210,8 → 208,8
break;
}
 
nick_rad = ((double)nick) * M_PI / (double)(1800.0);
roll_rad = ((double)roll) * M_PI / (double)(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);
/trunk/uart.c
98,16 → 98,11
uint8_t ConfirmFrame = 0;
uint8_t RemoteKeys = 0;
uint8_t DisplayLine = 0;
uint8_t DisplayBuff[80];
/*
const uint8_t DisplayBuff[4][20] =
{
"++ MK3MAG ++",
" ",
" ",
" "
};*/
 
const uint8_t BoardString[20] = "++ MK3MAG ++";
const uint8_t EmptyString[20] = " ";
const uint8_t VersionString[20] = {' ',' ',' ',' ',' ',' ',' ','V',(0x30 + VERSION_MAJOR),'.',('0' + VERSION_MINOR/10),('0' + VERSION_MINOR%10),('a' + VERSION_PATCH),' ',' ',' ',' ',' ',' ',' '};
 
uint16_t PC_Connected = 0;
uint16_t FC_Connected = 0;
 
163,7 → 158,6
/****************************************************************/
void USART0_Init (void)
{
uint8_t i;
uint8_t sreg = SREG;
uint16_t ubrr = (uint16_t) ((uint32_t) F_CPU/(8 * BAUD_RATE) - 1);
 
244,33 → 238,13
USART0_putchar ('P');
USART0_putchar (':');
USART0_putchar ('V');
USART0_putchar (0x30 + VERSION_MAJOR);
USART0_putchar ('0' + VERSION_MAJOR);
USART0_putchar ('.');
USART0_putchar (0x30 + VERSION_MINOR/10);
USART0_putchar (0x30 + VERSION_MINOR%10);
USART0_putchar ('0' + VERSION_MINOR/10);
USART0_putchar ('0' + VERSION_MINOR%10);
USART0_putchar ('a' + VERSION_PATCH);
USART0_putchar ('\n');
 
// prepare display buffer
for(i=0; i < 80; i++) DisplayBuff[i] = ' ';
DisplayBuff[0] = '+';
DisplayBuff[1] = '+';
DisplayBuff[7] = 'M';
DisplayBuff[8] = 'K';
DisplayBuff[9] = '3';
DisplayBuff[10] = 'M';
DisplayBuff[11] = 'A';
DisplayBuff[12] = 'G';
DisplayBuff[18] = '+';
DisplayBuff[19] = '+';
 
DisplayBuff[47] = 'V';
DisplayBuff[48] = (0x30 + VERSION_MAJOR);
DisplayBuff[49] = '.';
DisplayBuff[50] = (0x30 + VERSION_MINOR/10);
DisplayBuff[51] = (0x30 + VERSION_MINOR%10);
DisplayBuff[52] = ('a' + VERSION_PATCH);
 
// restore global interrupt flags
SREG = sreg;
}
646,7 → 620,20
 
if((RequestFlags & RQST_DISPLAY_DATA) && txd_complete)
{
SendOutData('H', MK3MAG_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), &DisplayBuff[DisplayLine * 20], 20);
switch(DisplayLine)
{
case 0:
SendOutData('H', MK3MAG_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), BoardString, 20);
break;
 
case 2:
SendOutData('H', MK3MAG_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), VersionString, 20);
break;
 
default:
SendOutData('H', MK3MAG_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), EmptyString, 20);
break;
}
DisplayLine++;
if(DisplayLine >= 4) DisplayLine = 0;
RequestFlags &= ~RQST_DISPLAY_DATA;
/trunk/uart.h
70,7 → 70,7
uint8_t Reserved[5];
} UART_VersionInfo_t;
 
extern uint8_t PC_Connected;
extern uint8_t FC_Connected;
extern uint16_t PC_Connected;
extern uint16_t FC_Connected;
 
#endif //_UART_H_