173,6 → 173,7 |
unsigned char Parameter_GlobalConfig; |
unsigned char Parameter_ExtraConfig; |
unsigned char Parameter_MaximumAltitude; |
unsigned char Parameter_LaserAltitude = 0; |
unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
unsigned char CareFree = 0; |
|
197,6 → 198,7 |
unsigned char ServoFailsafeActive = 0; // moves Servos into the FS-Position |
unsigned char Partner_StatusFlags = 0, Partner_StatusFlags2 = 0,Partner_StatusFlags3 = 0; |
signed char BaroExpandCnt = 0; // needed to detect a defctive baro-sensor |
signed long LaserAltitudeCorrection = 0; |
|
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
#define OPA_OFFSET_STEP 5 |
244,8 → 246,6 |
//DebugOut.Analog[16] = GasIsZeroCnt; |
//DebugOut.Analog[25] = Parameter_Hoehe_P; |
//DebugOut.Analog[26] = Parameter_Luftdruck_D; |
DebugOut.Analog[16] = FromNC_Laser_cm; |
DebugOut.Analog[17] = HoehenWertF; |
} |
|
|
560,6 → 560,64 |
} |
|
|
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
signed long LaserAltCorrection(void) |
{ |
static signed long laser_alt_correct = 0,laser_alt_correct_tmp = 0; |
static unsigned char laser_activated = 0; |
signed long diff; |
|
CHK_POTI(Parameter_LaserAltitude,EE_Parameter.LaserAltitudeParameter); |
if(GetChannelValue(EE_Parameter.LaserAltitudeChannel) < 50) Parameter_LaserAltitude = 0; |
|
DebugOut.Analog[26] = Parameter_LaserAltitude; |
//++++++++++++++++++++++++++++++++++++++++++++++++ |
if(Parameter_LaserAltitude > 5) |
{ |
if(laser_activated < 250) laser_activated++; |
} |
else |
if(Parameter_LaserAltitude == 0) |
{ |
laser_activated = 0; |
} |
//++++++++++++++++++++++++++++++++++++++++++++++++ |
if(laser_activated < 3) // wenn deaktiviert -> intern mitteln |
{ |
diff = FromNC_Laser_cm - (HoehenWertF + laser_alt_correct_tmp - laser_alt_correct); |
LIMIT_MIN_MAX(diff,-800,800); |
diff = diff/4; |
laser_alt_correct_tmp += diff; |
} |
else |
if(laser_activated == 3) // beim Aktivieren die Sollhöhe neu anlernen und Mittelwert als Korrekturwert übernehmen |
{ |
laser_activated++; |
laser_alt_correct = laser_alt_correct_tmp; |
LaserAltitudeCorrection = laser_alt_correct; // reset filter |
BaroExpandActive = 50; |
beeptime = 1000; |
} |
else |
if(laser_activated == 4) // beim Aktivieren die Sollhöhe neu anlernen und Mittelwert als Korrekturwert übernehmen |
{ |
SollHoehe = HoehenWertF; // update setpoint to current altitude if Laser gets activated |
} |
else // normale Regelung |
{ |
diff = FromNC_Laser_cm - HoehenWertF; |
LIMIT_MIN_MAX(diff,-400,400); |
diff = (diff * Parameter_LaserAltitude) / 512; |
laser_alt_correct_tmp += diff; |
laser_alt_correct = laser_alt_correct_tmp;//(laser_alt_correct + laser_alt_correct_tmp) / 2; |
} |
//++++++++++++++++++++++++++++++++++++++++++++++++ |
DebugOut.Analog[17] = laser_alt_correct_tmp; |
ToNC_LaserAltCorrect_dm = laser_alt_correct / 10; |
return(laser_alt_correct); |
} |
#endif |
|
//############################################################################ |
// Bearbeitet die Messwerte |
void Mittelwert(void) |
584,21 → 642,24 |
|
//++++++++++++++++++++++++++++++++++++++++++++++++ |
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
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; |
} |
|
HoehenWert = HoehenWert_Mess + LaserAltitudeCorrection; |
HoehenWertF = HoehenWertF_Mess + LaserAltitudeCorrection; |
|
if(FromNC_Laser_cm > 80 && FromNC_Laser_cm < 60000) |
{ |
if(!laser_correct_interval--) |
{ |
laser_alt_correct = LaserAltCorrection(); |
laser_correct_interval = 100; |
} |
else LaserAltitudeCorrection = (LaserAltitudeCorrection * 7 + laser_alt_correct)/8; |
} |
|
DebugOut.Analog[16] = FromNC_Laser_cm; |
DebugOut.Analog[18] = HoehenWertF; |
DebugOut.Analog[19] = HoehenWert; |
DebugOut.Analog[25] = HoehenWertF_Mess; |
|
#else |
HoehenWert = HoehenWert_Mess; |
HoehenWertF = HoehenWert; |