Subversion Repositories MK3Mag

Compare Revisions

Ignore whitespace Rev 31 → Rev 30

/branches/MK3Mag V0.14 Code Redesign Killagreg/main.c
158,7 → 158,6
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
188,28 → 187,25
switch(AttitudeSource)
{
case ATTITUDE_SOURCE_I2C:
cli(); // stop interrupts
nick = I2C_WriteAttitude.Nick;
roll = I2C_WriteAttitude.Roll;
sei(); // start interrupts
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:
cli(); // stop interrupts
nick = ExternData.Attitude[NICK];
roll = ExternData.Attitude[ROLL];
sei(); // start interrupts
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:
nick = AccAttitudeNick;
roll = AccAttitudeRoll;
nick_rad = ((double)AccAttitudeNick) * M_PI / (double)(1800.0);
roll_rad = ((double)AccAttitudeRoll) * M_PI / (double)(1800.0);
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);
218,6 → 214,7
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
225,11 → 222,8
if (heading < 0) heading = -heading;
else heading = 360 - heading;
 
cli(); // stop interrupts
if(abs(heading) < 361) Heading = heading;
else (Heading = -1);
sei(); // start interrupts
}
 
 
239,8 → 233,7
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;
252,11 → 245,11
{
if(CheckDelay(Led_Timer) || (cal != calold))
{
if(blinkcount & 0x01) if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF;
else if(invert_blinking) LED_GRN_OFF; else LED_GRN_ON;
if(blinkcount & 0x01) 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);
270,7 → 263,7
}
else
{
if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF;
LED_GRN_OFF;
}
 
// calibration state machine
288,7 → 281,6
Calibration.AccX.Offset = RawAccX;
Calibration.AccY.Offset = RawAccY;
Calibration.AccZ.Offset = RawAccZ;
invert_blinking = 0;
break;
 
case 2: // 2nd step of calibration
297,12 → 289,10
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:
309,7 → 299,6
// find Min and Max of the Z-Sensor
if(UncalMagZ < Zmin) Zmin = UncalMagZ;
if(UncalMagZ > Zmax) Zmax = UncalMagZ;
invert_blinking = 1;
break;
 
case 5:
332,7 → 321,6
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 = 17
VERSION_MINOR = 16
 
VERSION_COMPATIBLE = 8 # PC-Kompatibilität
#-------------------------------------------------------------------