Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1266 → Rev 1267

/beta/Code Redesign killagreg/analog.c
340,7 → 340,7
if(++AirPressCount >= AIR_PRESSURE_SCALE)
{
#define AIRPRESSURE_FILTER_TIME 8
AirPressure = (AirPressure * (AIRPRESSURE_FILTER_TIME - 1) + tmpAirPressure + AIR_PRESSURE_SCALE * EXPANDBARO_ADC_SHIFT * ExpandBaro + AIRPRESSURE_FILTER_TIME/2)/AIRPRESSURE_FILTER_TIME;
AirPressure = (AirPressure * (AIRPRESSURE_FILTER_TIME - 1) + tmpAirPressure + (AIR_PRESSURE_SCALE * EXPANDBARO_ADC_SHIFT) * (int32_t)ExpandBaro + AIRPRESSURE_FILTER_TIME/2)/AIRPRESSURE_FILTER_TIME;
ReadingHeight = (StartAirPressure - AirPressure); // change of air pressure to ground
SumHeight -= SumHeight/8;
SumHeight += ReadingHeight;
/beta/Code Redesign killagreg/fc.c
1513,7 → 1513,7
if(ParamSet.GlobalConfig & CFG_AIRPRESS_SENSOR)
{
#define HOOVER_GAS_AVERAGE 4192 // 4192 * 2ms = 8.2s averaging
#define HEIGHT_CONTROL_GAS_AVERAGE 16 // 16 * 2ms= 32 msa averaging
#define HEIGHT_CONTROL_GAS_AVERAGE 8 // 8 * 2ms= 16 ms averaging
 
int16_t CosNick, CosRoll;
int16_t HeightControlGas;
1583,14 → 1583,45
{
int16_t HooverGas;
static int16_t HeightTrimming = 0;
static uint8_t HeightTrimmingActive = 0;
#define HEIGHT_TRIM_UP 0x01
#define HEIGHT_TRIM_DOWN 0x02
static uint8_t HeightTrimmingFlag = 0x00;
 
HooverGas = (int16_t)(HooverGasEstimation/HOOVER_GAS_AVERAGE);
#define HEIGHT_CONTROL_STICKTHRESHOLD 25 * STICK_GAIN
if( (MKFlags & MKFLAG_FLY) && ( (GasMixFraction > (HooverGas + HEIGHT_CONTROL_STICKTHRESHOLD) ) || (GasMixFraction < (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD) ) ) )
{
HeightTrimmingActive = 1;
HeightTrimming += (GasMixFraction - HooverGas)/STICK_GAIN;
 
if(MKFlags & MKFLAG_FLY) // trim setpoint only when flying
{ // gas stick is above hoover point
if(GasMixFraction > (HooverGas + HEIGHT_CONTROL_STICKTHRESHOLD) )
{
if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN)
{
HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN;
SetPointHeight = ReadingHeight; // update setpoint to current heigth
}
HeightTrimmingFlag |= HEIGHT_TRIM_UP;
HeightTrimming += abs(GasMixFraction - (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD))/STICK_GAIN;
} // gas stick is below hoover point
else if(GasMixFraction < (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD) )
{
if(HeightTrimmingFlag & HEIGHT_TRIM_UP)
{
HeightTrimmingFlag &= ~HEIGHT_TRIM_UP;
SetPointHeight = ReadingHeight; // update setpoint to current heigth
}
HeightTrimmingFlag |= HEIGHT_TRIM_DOWN;
HeightTrimming -= abs(GasMixFraction - (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD))/STICK_GAIN;
}
else
{
if(HeightTrimmingFlag & (HEIGHT_TRIM_UP|HEIGHT_TRIM_DOWN))
{
HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP|HEIGHT_TRIM_DOWN);
HeightTrimming = 0;
SetPointHeight = ReadingHeight; // update setpointto current heigth
}
}
// Trim height set point
if(abs(HeightTrimming) > 512)
{
SetPointHeight += (HeightTrimming * ParamSet.Height_Gain)/512; // move setpoint
1597,16 → 1628,7
HeightTrimming = 0;
BeepTime = 50;
}
}
else
{
if(HeightTrimmingActive)
{
HeightTrimming = 0;
SetPointHeight = ReadingHeight; // update setpointto current heigth
HeightTrimmingActive = 0;
}
}
} //if MKFlags & MKFLAG_FLY
 
if(HooverGas == 0) HeightControlGas = GasMixFraction; // take stick gas
else HeightControlGas = HooverGas; // take hoover gas
/beta/Code Redesign killagreg/timer0.c
72,7 → 72,7
volatile uint8_t SendSPI = 0;
#endif
 
//#define USE_BEEPER
#define USE_BEEPER
 
 
/*****************************************************/