175,6 → 175,7 |
unsigned char Parameter_MaximumAltitude; |
unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
unsigned char CareFree = 0; |
|
const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; // 15° steps |
|
int MaxStickNick = 0,MaxStickRoll = 0; |
241,9 → 242,10 |
//DebugOut.Analog[16] = Variance; |
//DebugOut.Analog[17] = VarioMeter; |
//DebugOut.Analog[16] = GasIsZeroCnt; |
//DebugOut.Analog[18] = HoehenWertF; |
//DebugOut.Analog[25] = Parameter_Hoehe_P; |
//DebugOut.Analog[26] = Parameter_Luftdruck_D; |
DebugOut.Analog[16] = FromNC_Laser_cm; |
DebugOut.Analog[17] = HoehenWertF; |
} |
|
|
563,9 → 565,10 |
void Mittelwert(void) |
//############################################################################ |
{ |
static signed long tmpl,tmpl2,tmpl3,tmpl4; |
static signed long tmpl,tmpl2,tmpl3,tmpl4, laser_alt_correct = 0; |
static signed int oldNick, oldRoll, d2Roll, d2Nick; |
signed long winkel_nick, winkel_roll; |
static unsigned char laser_correct_interval; |
MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
MesswertNick = (signed int) AdWertNickFilter / 8; |
MesswertRoll = (signed int) AdWertRollFilter / 8; |
580,10 → 583,24 |
NaviCntAcc++; |
|
//++++++++++++++++++++++++++++++++++++++++++++++++ |
HoehenWert = HoehenWert_Mess; |
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
HoehenWertF = HoehenWertF_Mess; |
HoehenWert = HoehenWert_Mess + laser_alt_correct; |
HoehenWertF = HoehenWertF_Mess + laser_alt_correct; |
DebugOut.Analog[26] = Parameter_UserParam8; |
if(FromNC_Laser_cm > 80 && FromNC_Laser_cm < 60000 && !laser_correct_interval--) |
{ |
signed long diff; |
diff = FromNC_Laser_cm - HoehenWertF; |
LIMIT_MIN_MAX(diff,-300,300); |
diff = (diff * Parameter_UserParam8) / 2048; |
laser_alt_correct += diff; |
DebugOut.Analog[18] = laser_alt_correct; |
laser_correct_interval = 100; |
ToNC_LaserAltCorrect_dm = laser_alt_correct / 10; |
} |
|
#else |
HoehenWert = HoehenWert_Mess; |
HoehenWertF = HoehenWert; |
#endif |
//++++++++++++++++++++++++++++++++++++++++++++++++ |