106,8 → 106,10 |
int16_t AccAttitudeNick = 0, AccAttitudeRoll = 0; // nick and roll angle from acc |
|
int16_t Heading = -1; // the current compass heading in deg |
int16_t ZeroHeading = 180; |
unsigned char InternalCalstate = 0, ActualCalstate = 0; |
signed char PotiToFC[8] = {10,20,30,40,50,60,70,80}; |
|
|
void CalcFields(void) |
{ |
UncalMagX = (RawMagnet1a - RawMagnet1b); |
119,7 → 121,7 |
else MagY = 0; |
if(Calibration.MagY.Range != 0) MagZ = (1024L * (int32_t)(UncalMagZ - Calibration.MagZ.Offset)) / (Calibration.MagZ.Range); |
else MagZ = 0; |
if(AccPresent) |
if(AttitudeSource == ATTITUDE_SOURCE_ACC) |
{ |
AccX = (RawAccX - Calibration.AccX.Offset); |
AccY = (RawAccY - Calibration.AccY.Offset); |
238,28 → 240,22 |
|
void Calibrate(void) |
{ |
uint8_t cal; |
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; |
|
|
if(cal > 5) cal = 0; |
if(ActualCalstate > 5) ActualCalstate = 0; |
// blink code for current calibration state |
if(cal) |
if(ActualCalstate) |
{ |
if(CheckDelay(Led_Timer) || (cal != calold)) |
if(CheckDelay(Led_Timer) || (ActualCalstate != 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; |
|
// end of blinkcount sequence |
if((blinkcount + 1 ) >= (2 * cal)) |
if((blinkcount + 1 ) >= (2 * ActualCalstate)) |
{ |
blinkcount = 0; |
Led_Timer = SetDelay(1500); |
276,7 → 272,7 |
if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF; |
} |
// calibration state machine |
switch(cal) |
switch(ActualCalstate) |
{ |
case 1: // 1st step of calibration |
// initialize ranges |
316,7 → 312,7 |
|
case 5: |
// Save values |
if(cal != calold) // avoid continously writing of eeprom! |
if(ActualCalstate != calold) // avoid continously writing of eeprom! |
{ |
Calibration.MagX.Range = Xmax - Xmin; |
Calibration.MagX.Offset = (Xmin + Xmax) / 2; |
346,7 → 342,7 |
default: |
break; |
} |
calold = cal; |
calold = ActualCalstate; |
} |
|
|
369,12 → 365,14 |
DebugOut.Analog[6] = ExternData.Attitude[NICK]; |
DebugOut.Analog[7] = ExternData.Attitude[ROLL]; |
break; |
|
|
case ATTITUDE_SOURCE_I2C: |
DebugOut.Analog[6] = I2C_WriteAttitude.Nick; |
DebugOut.Analog[7] = I2C_WriteAttitude.Roll; |
break; |
default: |
DebugOut.Analog[6] = -1; |
DebugOut.Analog[7] = -1; |
break; |
} |
DebugOut.Analog[8] = Calibration.MagX.Offset; |
DebugOut.Analog[9] = Calibration.MagX.Range; |
382,11 → 380,10 |
DebugOut.Analog[11] = Calibration.MagY.Range; |
DebugOut.Analog[12] = Calibration.MagZ.Offset; |
DebugOut.Analog[13] = Calibration.MagZ.Range; |
if(I2C_WriteCal.CalByte) DebugOut.Analog[14] = I2C_WriteCal.CalByte; |
else DebugOut.Analog[14] = ExternData.CalState; |
DebugOut.Analog[14] = ActualCalstate; |
DebugOut.Analog[15] = Heading; |
DebugOut.Analog[16] = ExternData.UserParam[0]; |
DebugOut.Analog[17] = ExternData.UserParam[1]; |
DebugOut.Analog[16] = 0; |
DebugOut.Analog[17] = 0; |
DebugOut.Analog[18] = AccX; |
DebugOut.Analog[19] = AccY; |
DebugOut.Analog[20] = AccZ; |
393,9 → 390,9 |
DebugOut.Analog[21] = RawAccX; |
DebugOut.Analog[22] = RawAccY; |
DebugOut.Analog[23] = RawAccZ; |
DebugOut.Analog[24] = Calibration.AccX.Offset; |
DebugOut.Analog[25] = Calibration.AccY.Offset; |
DebugOut.Analog[26] = Calibration.AccZ.Offset; |
DebugOut.Analog[24] = PotiToFC[0]; |
DebugOut.Analog[25] = PotiToFC[1]; |
DebugOut.Analog[26] = PotiToFC[2]; |
DebugOut.Analog[29] = AttitudeSource; |
} |
|
415,6 → 412,28 |
} |
} |
|
#ifdef HEADTRACKER |
void CalcFcPotis(void) |
{ |
signed int tmp0, tmp1, tmp2; |
if(AccPresent) |
{ |
tmp0 = AccAttitudeNick/4; |
tmp1 = AccAttitudeRoll/4; |
|
tmp2 = (360 + Heading - ZeroHeading) % 360; |
tmp2 /= 2; |
tmp2 -= 90; |
if(tmp0 > 127) tmp0 = 127; else if(tmp0 < -127) tmp0 = -127; |
if(tmp1 > 127) tmp1 = 127; else if(tmp1 < -127) tmp1 = -127; |
if(tmp2 > 127) tmp2 = 127; else if(tmp2 < -127) tmp2 = -127; |
PotiToFC[0] = tmp0; |
PotiToFC[1] = tmp1; |
PotiToFC[2] = tmp2; |
} |
} |
#endif |
|
int main (void) |
{ |
static uint8_t state = 0; |
431,12 → 450,17 |
|
sei(); // enable globale interrupts |
USART0_Print("\n\rthe use of this software is only permitted\n\ron original MikroKopter-Hardware"); |
USART0_Print("\n\rwww.MikroKopter.de (c) HiSystems GmbH"); |
|
USART0_Print("\n\rwww.MikroKopter.de (c) HiSystems GmbH\n\n"); |
#ifdef HEADTRACKER |
USART0_Print("Head-Tracker\n"); |
#endif |
if(AccPresent) |
{ |
USART0_Print("\n\nACC present\n"); |
USART0_Print("ACC present\n"); |
} |
#ifdef HEADTRACKER |
else USART0_Print("\nERROR: NO ACC\n"); |
#endif |
|
LED_GRN_ON; |
|
445,11 → 469,11 |
// read calibration info from eeprom |
eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration)); |
|
InternalCalstate = 0; |
ExternData.CalState = 0; |
I2C_WriteCal.CalByte = 0; |
|
ActualCalstate = 0; |
statetimer = SetDelay(0); |
|
// main loop |
while (1) |
{ |
495,19 → 519,29 |
AccMeasurement(); |
statetimer = SetDelay(1); |
state = 0; |
|
CalcFields(); |
if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate(); |
// check both sources of communication for calibration request |
if(InternalCalstate) ActualCalstate = InternalCalstate; |
else |
if(I2C_WriteCal.CalByte) ActualCalstate = I2C_WriteCal.CalByte; |
else ActualCalstate = ExternData.CalState; |
if(ActualCalstate) 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 |
#ifdef HEADTRACKER |
CalcFcPotis(); |
if(AccPresent) AttitudeSource = ATTITUDE_SOURCE_ACC; |
#else |
if(!FC_Connected && ! NC_Connected) |
{ |
AttitudeSource = ATTITUDE_SOURCE_ACC; |
if(AccPresent) AttitudeSource = ATTITUDE_SOURCE_ACC; |
else AttitudeSource = ATTITUDE_SOURCE_NONE; |
Orientation = ORIENTATION_FC; |
} |
#endif |
} |
break; |
|
519,7 → 553,9 |
|
// check data from USART |
USART0_ProcessRxData(); |
|
#ifdef HEADTRACKER |
USART0_TransmitTxData(); |
#else |
if(PC_Connected) |
{ |
USART0_EnableTXD(); |
529,6 → 565,7 |
{ |
USART0_DisableTXD(); |
} |
#endif |
} // while(1) |
} |
|