Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2238 → Rev 2239

/test_branches/FC2_2/analog.c
70,7 → 70,7
volatile unsigned char AdReady = 1;
volatile long HoehenWertF = 0, HoehenDiff, HoeheAlt;
volatile long vv, vvSum;
signed int AccZSum = 0;
//signed int AccZSum = 0;
 
//#######################################################################################
void ADC_Init(void)
244,8 → 244,9
//if(PlatinenVersion < 22)
//else Aktuell_az = 1024 - ADC;
// tmp = (signed int) Aktuell_az - NeutralAccZ;
AccZSum = AccZSum - AdWertAccHoch + Aktuell_az - NeutralAccZ;
AdWertAccHoch = AccZSum/32;
AdWertAccHoch = Aktuell_az - NeutralAccZ;
// AccZSum = AccZSum - AdWertAccHoch + Aktuell_az - NeutralAccZ;
// AdWertAccHoch = AccZSum/32;
// AdWertAccHoch = (7 * AdWertAccHoch + tmp) / 8;
}
if(PlatinenVersion < 22)
326,13 → 327,13
J17_ON; //Qopter: testweise zur Rechenzeitmessung
HoehenDiff = HoehenWert - HoeheAlt;
// vvSum = vvSum - vv + ((Aktuell_az - NeutralAccZ) - AccShorter)*7 + HoehenDiff*500; // Fusion vert. velocity
vvSum = vvSum - vv + (Aktuell_az - NeutralAccZ)*7 + HoehenDiff*500; // Fusion vert. velocity
vvSum = vvSum - vv + AdWertAccHoch*7 + HoehenDiff*500; // Fusion vert. velocity
vv = (vvSum + 256)/512; // cm/s
HoeheAlt = HoehenWert;
SummenHoehe = SummenHoehe - HoehenWertF + vv + HoehenWert; //Fusion Hoehe
HoehenWertF = (SummenHoehe + SM_FILTER/2)/SM_FILTER; // cm
J17_OFF;
DebugOut.Analog[19] = (Aktuell_az - NeutralAccZ);
//DebugOut.Analog[19] = AdWertAccHoch*7;
//DebugOut.Analog[16] = ((Aktuell_az - NeutralAccZ) - AccShorter);
state = 0;
AdReady = 1;
345,7 → 346,7
Luftdruck += tmpLuftdruck;
HoehenWert = (StartLuftdruck - Luftdruck)/1; // cm
J17_ON; //Qopter: testweise zur Rechenzeitmessung
if(PlatinenVersion > 21) HoehenWert = HoehenWert/3 + AdWertAccHoch*41/256; //+ compensates disturbance due to acceleration
if(PlatinenVersion > 21) HoehenWert = HoehenWert*5/16 + AdWertAccHoch*41/256; //+ compensates disturbance due to acceleration
J17_OFF;
/* // Qopter: deaktivieren Anfang
if(PlatinenVersion > 21 && NeutralAccZ) MessLuftdruck -= (Aktuell_az - NeutralAccZ)/12;
/test_branches/FC2_2/fc.c
192,7 → 192,7
DebugOut.Analog[4] = (signed int) AdNeutralGier - AdWertGier;
DebugOut.Analog[5] = HoehenWert;///5;
DebugOut.Analog[6] = Aktuell_az;//AdWertAccHoch;//(Mess_Integral_Hoch / 512);
DebugOut.Analog[8] = AdWertAccHoch/200;//KompassValue;
DebugOut.Analog[8] = AdWertAccHoch;//KompassValue;
DebugOut.Analog[9] = UBat;
DebugOut.Analog[10] = SenderOkay;
DebugOut.Analog[11] = ErsatzKompassInGrad;
1498,7 → 1498,7
if(HoehenReglerAktiv && !SpeakHoTT) SpeakHoTT = SPEAK_ALTITUDE_OFF;
#endif
HoehenReglerAktiv = 0; // disable height control
SollHoehe = HoehenWert; // update SetPoint with current reading
SollHoehe = HoehenWertF; // update SetPoint with current reading
delay = 1;
}
}
1558,7 → 1558,7
if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN)
{
FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN;
SollHoehe = HoehenWert; // update setpoint to current heigth
SollHoehe = HoehenWertF; // update setpoint to current heigth
}
FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP;
// Limit the maximum Altitude
1577,7 → 1577,7
if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP)
{
FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP;
SollHoehe = HoehenWert; // update setpoint to current heigth
SollHoehe = HoehenWertF; // update setpoint to current heigth
}
FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN;
AltitudeSetpointTrimming = -abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD));
1598,7 → 1598,7
if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) // changed from sinking to rising
{
FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN;
SollHoehe = HoehenWert; // update setpoint to current heigth
SollHoehe = HoehenWertF; // update setpoint to current heigth
}
}
else
1612,13 → 1612,13
if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) // changed from rising to sinking
{
FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP;
SollHoehe = HoehenWert; // update setpoint to current heigth
SollHoehe = HoehenWertF; // update setpoint to current heigth
}
}
else
if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN))
{
if(!WaypointTrimming) LIMIT_MIN_MAX(SollHoehe, (HoehenWert-128), (HoehenWert+128)) // max. 1m Unterschied
if(!WaypointTrimming) LIMIT_MIN_MAX(SollHoehe, (HoehenWert-200), (HoehenWert+200)) // max. 2m Unterschied
else WaypointTrimming = 0;
FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN);
HeightTrimming = 0;
1655,7 → 1655,7
else if(StickGasHover > 150) StickGasHover = 150;
}
}
if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active
if(BaroExpandActive) SollHoehe = HoehenWertF; // update setpoint to current altitude if Expanding is active
} //if FCFlags & MKFCFLAG_FLY
else
{
1667,7 → 1667,7
}
HCGas = HoverGas; // take hover gas (neutral point)
}
if(HoehenWert > SollHoehe || !(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT))
if(HoehenWertF > SollHoehe || !(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT))
{
// from this point the Heigth Control Algorithm is identical for both versions
if(BaroExpandActive) // baro range expanding active
1842,11 → 1842,11
FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN);
} // EOF no height control
 
// Linits the maximum gas in case of "Out of Range emergency landing"
// Limits the maximum gas in case of "Out of Range emergency landing"
if(NC_To_FC_Flags & NC_TO_FC_EMERGENCY_LANDING)
{
if(GasMischanteil/STICK_GAIN > HooverGasEmergencyPercent && HoverGas) GasMischanteil = HooverGasEmergencyPercent * STICK_GAIN;
SollHoehe = HoehenWert; // update setpoint to current heigth
SollHoehe = HoehenWertF; // update setpoint to current heigth
beeptime = 15000;
BeepMuster = 0x0E00;
}