Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2755 → Rev 2756

/trunk/fc.c
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;