Subversion Repositories MK3Mag

Compare Revisions

Ignore whitespace Rev 30 → Rev 31

/branches/MK3Mag V0.14 Code Redesign Killagreg/main.c
158,6 → 158,7
void CalcHeading(void)
{
double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0;
int16_t nick, roll;
int16_t heading = -1;
 
// blink code for normal operation
187,25 → 188,28
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);
cli(); // stop interrupts
nick = I2C_WriteAttitude.Nick;
roll = I2C_WriteAttitude.Roll;
sei(); // start interrupts
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);
cli(); // stop interrupts
nick = ExternData.Attitude[NICK];
roll = ExternData.Attitude[ROLL];
sei(); // start interrupts
break;
 
case ATTITUDE_SOURCE_ACC:
nick_rad = ((double)AccAttitudeNick) * M_PI / (double)(1800.0);
roll_rad = ((double)AccAttitudeRoll) * M_PI / (double)(1800.0);
nick = AccAttitudeNick;
roll = AccAttitudeRoll;
break;
 
default:
nick_rad = 0;
roll_rad = 0;
break;
}
nick_rad = ((double)nick) * M_PI / (double)(1800.0);
roll_rad = ((double)roll) * M_PI / (double)(1800.0);
 
// calculate attitude correction
Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad);
214,7 → 218,6
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
222,8 → 225,11
if (heading < 0) heading = -heading;
else heading = 360 - heading;
 
cli(); // stop interrupts
if(abs(heading) < 361) Heading = heading;
else (Heading = -1);
sei(); // start interrupts
}
 
 
233,7 → 239,8
static uint8_t calold = 0;
static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
static uint8_t blinkcount = 0;
 
static uint8_t invert_blinking = 0;
// check both sources of communication for calibration request
if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;
else cal = ExternData.CalState;
245,11 → 252,11
{
if(CheckDelay(Led_Timer) || (cal != calold))
{
if(blinkcount & 0x01) LED_GRN_OFF;
else LED_GRN_ON;
if(blinkcount & 0x01) if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF;
else if(invert_blinking) LED_GRN_OFF; else LED_GRN_ON;
 
// end of blinkcount sequence
if( (blinkcount + 1 ) >= (2 * cal) )
if((blinkcount + 1 ) >= (2 * cal))
{
blinkcount = 0;
Led_Timer = SetDelay(1000);
263,7 → 270,7
}
else
{
LED_GRN_OFF;
if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF;
}
 
// calibration state machine
281,6 → 288,7
Calibration.AccX.Offset = RawAccX;
Calibration.AccY.Offset = RawAccY;
Calibration.AccZ.Offset = RawAccZ;
invert_blinking = 0;
break;
 
case 2: // 2nd step of calibration
289,10 → 297,12
if(UncalMagX > Xmax) Xmax = UncalMagX;
if(UncalMagY < Ymin) Ymin = UncalMagY;
if(UncalMagY > Ymax) Ymax = UncalMagY;
invert_blinking = 1;
break;
 
case 3: // 3rd step of calibration
// used to change the orientation of the MK3MAG vertical to the horizontal plane
invert_blinking = 0;
break;
 
case 4:
299,6 → 309,7
// find Min and Max of the Z-Sensor
if(UncalMagZ < Zmin) Zmin = UncalMagZ;
if(UncalMagZ > Zmax) Zmax = UncalMagZ;
invert_blinking = 1;
break;
 
case 5:
321,6 → 332,7
blinkcount = 0;
}
}
invert_blinking = 0;
break;
 
default:
/branches/MK3Mag V0.14 Code Redesign Killagreg/makefile
6,7 → 6,7
BOARD = 10
#BOARD = 11
VERSION_MAJOR = 0
VERSION_MINOR = 16
VERSION_MINOR = 17
 
VERSION_COMPATIBLE = 8 # PC-Kompatibilit├Ąt
#-------------------------------------------------------------------