107,8 → 107,8 |
|
int16_t Heading = -1; // the current compass heading in deg |
int16_t ZeroHeading = 180; |
uint8_t InternalCalstate = 0, ActualCalstate = 0; |
int8_t PotiToFC[8] = {10,20,30,40,50,60,70,80}; |
unsigned char InternalCalstate = 0, ActualCalstate = 0; |
signed char PotiToFC[12] = {0,0,0,0,0,0,0,0,0,0,0,0}; |
|
void CalcFields(void) |
{ |
188,7 → 188,9 |
Cz = (double)MagZ; |
break; |
} |
|
#ifdef HEADTRACKER |
AttitudeSource = ATTITUDE_SOURCE_ACC; |
#endif |
// calculate nick and roll angle in rad |
switch(AttitudeSource) |
{ |
418,19 → 420,18 |
int16_t 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; |
} |
tmp0 = AccAttitudeNick/4; |
tmp1 = AccAttitudeRoll/4; |
tmp2 = (360 + Heading - ZeroHeading) % 360; |
tmp2 -= 180; |
tmp2 *= 2; |
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] = (PotiToFC[0] + tmp0) / 2; |
PotiToFC[1] = (PotiToFC[1] + tmp1) / 2; |
PotiToFC[2] = (PotiToFC[2] + tmp2) / 2; |
} |
} |
#endif |
|