Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1330 → Rev 1331

/beta/Code Redesign killagreg/fc.c
668,8 → 668,11
static uint16_t UpdateCompassCourse = 0;
// high resolution motor values for smoothing of PID motor outputs
static int16_t MotorValue[MAX_MOTORS];
static uint8_t LoadHandler = 5;
uint8_t i;
 
if(--LoadHandler == 0) LoadHandler = 5; // distribution of proccessor load
 
Mean();
GRN_ON;
 
1427,7 → 1430,7
#endif
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// DebugOutputs
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!TimerDebugOut--)
{
1494,7 → 1497,6
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Height Control
// The height control algorithm reduces the gas but does not increase the gas.
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GasMixFraction = StickGas; // take the direct stick command
// at full LiPo the voltage is higher that gives more trust at the same BL-Control settpoint
1509,313 → 1511,323
// if airpressure sensor is enabled
if((ParamSet.Config0 & CFG0_AIRPRESS_SENSOR) && !(LoopingRoll || LoopingNick) )
{
#define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging
#define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging
static int16_t FilterHCGas = 0;
 
int16_t CosAttitude; // for projection of hoover gas
if(LoadHandler == 1) // only every 5th run
{ // 2ms * 5 = 10ms update
#define HOOVER_GAS_AVERAGE 1024L // 1024 * 10ms = 10s averaging
#define HC_GAS_AVERAGE 2 // 2 * 10ms= 20 ms averaging
 
int16_t HCGas, HeightDeviation;
static int16_t HeightTrimming = 0; // rate for change of height setpoint
static uint8_t HCActive = 0;
static int16_t FilterHCGas = 0;
static int16_t StickGasHoover = RC_GAS_OFFSET, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023;
static uint32_t HooverGasFilter = 0;
static uint8_t delay = 100;
int16_t CosAttitude; // for projection of hoover gas
int16_t HCGas, HeightDeviation;
static int16_t HeightTrimming = 0; // rate for change of height setpoint
static uint8_t HCActive = 0;
static int16_t StickGasHoover = RC_GAS_OFFSET, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023;
static uint32_t HooverGasFilter = 0;
static uint8_t delay = 100;
 
#define BARO_LIMIT_MAX 0x01
#define BARO_LIMIT_MIN 0x02
static uint8_t BaroFlags = 0;
static uint8_t BaroExpandActive = 0;
#define BARO_LIMIT_MAX 0x01
#define BARO_LIMIT_MIN 0x02
static uint8_t BaroFlags = 0;
static uint8_t BaroExpandActive = 0;
 
// get the current hooverpoint
DebugOut.Analog[21] = HooverGas;
DebugOut.Analog[18] = ReadingVario;
// get the current hooverpoint
DebugOut.Analog[21] = HooverGas;
DebugOut.Analog[18] = ReadingVario;
 
// if height control is activated by an rc channel
if(ParamSet.Config0 & CFG0_HEIGHT_SWITCH)
{ // check if parameter is less than activation threshold
if( FCParam.MaxHeight < 50 ) // for 3 or 2-state switch height control is disabled in lowest position
{ //height control not active
if(!delay--)
{
 
SetPointHeight = ReadingHeight; // update SetPoint with current reading
HCActive = 0; // disable height control
delay = 1;
}
// --------- barometer range expansion ------------------
if(BaroExpandActive) // delay, because of expanding the Baro-Range
{
SumHeight = ReadingHeight * SM_FILTER; // reinit filter for vario
ReadingVario = 0;
// count down
BaroExpandActive--;
}
else
{ //height control is activated
HCActive = 1; // enable height control
delay = 200;
}
}
else // no switchable height control
{ // the height control is always active and the set point is defined by the parameter
if( !(BaroFlags & (BARO_LIMIT_MIN|BARO_LIMIT_MAX)) )
else // expansion not active
{
SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain;
}
HCActive = 1;
}
// measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs
if(AdAirPressure > 923)
{ // increase offset
if(OCR0A < (255 - EXPANDBARO_OPA_OFFSET_STEP))
{
ExpandBaro -= 1;
OCR0A = PressureSensorOffset - EXPANDBARO_OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down
BeepTime = 300;
BaroExpandActive = 50;
}
else
{
BaroFlags |= BARO_LIMIT_MIN;
}
}
// measurement of air pressure close to lower limit and
else if(AdAirPressure < 100 )
{ // decrease offset
if(OCR0A > EXPANDBARO_OPA_OFFSET_STEP)
{
ExpandBaro += 1;
OCR0A = PressureSensorOffset - EXPANDBARO_OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up
BeepTime = 300;
BaroExpandActive = 50;
}
else
{
BaroFlags |= BARO_LIMIT_MAX;
}
}
else
{ // still ok
BaroFlags &= ~(BARO_LIMIT_MIN | BARO_LIMIT_MAX);
}
}// EOF --------- barometer range expansion ------------------
 
// --------- barometer range expansion ------------------
if(BaroExpandActive) // delay, because of expanding the Baro-Range
{
SumHeight = ReadingHeight * SM_FILTER; // reinit filter for vario
ReadingVario = 0;
// count down
BaroExpandActive--;
}
else // expansion not active
{
// measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs
if(AdAirPressure > 923)
{ // increase offset
if(OCR0A < (255 - EXPANDBARO_OPA_OFFSET_STEP))
{
ExpandBaro -= 1;
OCR0A = PressureSensorOffset - EXPANDBARO_OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down
BeepTime = 300;
BaroExpandActive = 250;
 
 
// if height control is activated by an rc channel
if(ParamSet.Config0 & CFG0_HEIGHT_SWITCH)
{ // check if parameter is less than activation threshold
if( FCParam.MaxHeight < 50 ) // for 3 or 2-state switch height control is disabled in lowest position
{ //height control not active
if(!delay--)
{
 
SetPointHeight = ReadingHeight; // update SetPoint with current reading
HCActive = 0; // disable height control
delay = 1;
}
}
else
{
BaroFlags |= BARO_LIMIT_MIN;
{ //height control is activated
HCActive = 1; // enable height control
delay = 40;
}
}
// measurement of air pressure close to lower limit and
else if(AdAirPressure < 100 )
{ // decrease offset
if(OCR0A > EXPANDBARO_OPA_OFFSET_STEP)
else // no switchable height control
{ // the height control is always active and the set point is defined by the parameter
if( !(BaroFlags & (BARO_LIMIT_MIN|BARO_LIMIT_MAX)) )
{
ExpandBaro += 1;
OCR0A = PressureSensorOffset - EXPANDBARO_OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up
BeepTime = 300;
BaroExpandActive = 250;
SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain;
}
else
{
BaroFlags |= BARO_LIMIT_MAX;
}
HCActive = 1;
}
else
{ // still ok
BaroFlags &= ~(BARO_LIMIT_MIN | BARO_LIMIT_MAX);
}
}// EOF --------- barometer range expansion ------------------
 
 
// calculate cos of nick and roll angle used for projection of the vertical hoover gas
tmp_int = (int16_t)(IntegralGyroNick/GYRO_DEG_FACTOR); // nick angle in deg
tmp_int2 = (int16_t)(IntegralGyroRoll/GYRO_DEG_FACTOR); // roll angle in deg
CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2);
DebugOut.Analog[22] = CosAttitude;
LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle
CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude
// calculate cos of nick and roll angle used for projection of the vertical hoover gas
tmp_int = (int16_t)(IntegralGyroNick/GYRO_DEG_FACTOR); // nick angle in deg
tmp_int2 = (int16_t)(IntegralGyroRoll/GYRO_DEG_FACTOR); // roll angle in deg
CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2);
LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle
CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude
 
if(HCActive && !(MKFlags & MKFLAG_EMERGENCY_LANDING))
{
if((ParamSet.Config2 & CFG2_HEIGHT_LIMIT) || !(ParamSet.Config0 & CFG0_HEIGHT_SWITCH))
if(HCActive && !(MKFlags & MKFLAG_EMERGENCY_LANDING))
{
// Holger original version
// start of height control algorithm
// the height control is only an attenuation of the actual gas stick.
// I.e. it will work only if the gas stick is higher than the hover gas
// and the hover height will be allways larger than height setpoint.
if((ParamSet.Config2 & CFG2_HEIGHT_LIMIT) || !(ParamSet.Config0 & CFG0_HEIGHT_SWITCH))
{
// Holger original version
// start of height control algorithm
// the height control is only an attenuation of the actual gas stick.
// I.e. it will work only if the gas stick is higher than the hover gas
// and the hover height will be allways larger than height setpoint.
 
HCGas = GasMixFraction; // take current stick gas as neutral point for the height control
HeightTrimming = 0;
}
else // alternative height control
{
// PD-Control with respect to hoover point
// the setpoint will be fine adjusted with the gas stick position
#define HC_TRIM_UP 0x01
#define HC_TRIM_DOWN 0x02
static uint8_t HeightTrimmingFlag = 0x00;
HCGas = GasMixFraction; // take current stick gas as neutral point for the height control
HeightTrimming = 0;
}
else // alternative height control
{
// PD-Control with respect to hoover point
// the setpoint will be fine adjusted with the gas stick position
#define HC_TRIM_UP 0x01
#define HC_TRIM_DOWN 0x02
static uint8_t HeightTrimmingFlag = 0x00;
 
#define HC_STICKTHRESHOLD 15
#define HC_STICKTHRESHOLD 15
 
if(MKFlags & MKFLAG_FLY) // trim setpoint only when flying
{ // gas stick is above hoover point
if(StickGas > (StickGasHoover + HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MAX))
{
if(HeightTrimmingFlag & HC_TRIM_DOWN)
if(MKFlags & MKFLAG_FLY) // trim setpoint only when flying
{ // gas stick is above hoover point
if(StickGas > (StickGasHoover + HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MAX))
{
HeightTrimmingFlag &= ~HC_TRIM_DOWN;
SetPointHeight = ReadingHeight; // update setpoint to current height
if(HeightTrimmingFlag & HC_TRIM_DOWN)
{
HeightTrimmingFlag &= ~HC_TRIM_DOWN;
SetPointHeight = ReadingHeight; // update setpoint to current height
}
HeightTrimmingFlag |= HC_TRIM_UP;
HeightTrimming += abs(StickGas - (StickGasHoover + HC_STICKTHRESHOLD));
} // gas stick is below hoover point
else if(StickGas < (StickGasHoover - HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MIN))
{
if(HeightTrimmingFlag & HC_TRIM_UP)
{
HeightTrimmingFlag &= ~HC_TRIM_UP;
SetPointHeight = ReadingHeight; // update setpoint to current heigth
}
HeightTrimmingFlag |= HC_TRIM_DOWN;
HeightTrimming -= abs(StickGas - (StickGasHoover - HC_STICKTHRESHOLD));
}
HeightTrimmingFlag |= HC_TRIM_UP;
HeightTrimming += abs(StickGas - (StickGasHoover + HC_STICKTHRESHOLD));
} // gas stick is below hoover point
else if(StickGas < (StickGasHoover - HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MIN))
{
if(HeightTrimmingFlag & HC_TRIM_UP)
else // gas stick in hoover range
{
HeightTrimmingFlag &= ~HC_TRIM_UP;
SetPointHeight = ReadingHeight; // update setpoint to current heigth
if(HeightTrimmingFlag & (HC_TRIM_UP|HC_TRIM_DOWN))
{
HeightTrimmingFlag &= ~(HC_TRIM_UP|HC_TRIM_DOWN);
HeightTrimming = 0;
SetPointHeight = ReadingHeight; // update setpoint to current height
if(ParamSet.Config2 & CFG2_VARIO_BEEP) BeepTime = 500;
}
}
HeightTrimmingFlag |= HC_TRIM_DOWN;
HeightTrimming -= abs(StickGas - (StickGasHoover - HC_STICKTHRESHOLD));
}
else // gas stick in hoover range
{
if(HeightTrimmingFlag & (HC_TRIM_UP|HC_TRIM_DOWN))
// trim height set point if needed
if(abs(HeightTrimming) > 256)
{
HeightTrimmingFlag &= ~(HC_TRIM_UP|HC_TRIM_DOWN);
SetPointHeight += (HeightTrimming * ParamSet.Height_Gain)/256; // move setpoint
HeightTrimming = 0;
SetPointHeight = ReadingHeight; // update setpoint to current height
if(ParamSet.Config2 & CFG2_VARIO_BEEP) BeepTime = 500;
}
if(ParamSet.Config2 & CFG2_VARIO_BEEP) BeepTime = 75;
/*
//update hoover gas stick value when setpoint is shifted
if(!ParamSet.Height_StickNeutralPoint)
{
StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value
StickGasHoover = (StickGasHoover * UBat) / LowVoltageWarning;
LIMIT_MIN_MAX(StickGasHoover, 70, 150); // reserve some range for trim up and down
}*/
} // EOF trimming height set point
} //if MKFlags & MKFLAG_FLY
else // not flying but height control is already active
{
SetPointHeight = ReadingHeight - 400; // setpoint should be 4 meters below actual height to avoid a take off
if(ParamSet.Height_StickNeutralPoint) StickGasHoover = ParamSet.Height_StickNeutralPoint;
else StickGasHoover = RC_GAS_OFFSET;
}
// trim height set point if needed
if(abs(HeightTrimming) > 512)
 
HCGas = HooverGas; // take hoover gas (neutral point for PD controller)
 
} //EOF alternative height control
 
if((ReadingHeight > SetPointHeight) || !(ParamSet.Config2 & CFG2_HEIGHT_LIMIT) )
{
// from this point the Heigth Control Algorithm is identical for both versions
if(BaroExpandActive) // baro range expanding active
{
SetPointHeight += (HeightTrimming * ParamSet.Height_Gain)/(( 512 * 5 ) / 2); // move setpoint
HeightTrimming = 0;
if(ParamSet.Config2 & CFG2_VARIO_BEEP) BeepTime = 75;
//update hoover gas stick value when setpoint is shifted
if(!ParamSet.Height_StickNeutralPoint)
HCGas = HooverGas; // hoover while expanding baro adc range
} // EOF // baro range expanding active
else // no baro range expanding
{
// ------------------------- P-Part ----------------------------
HeightDeviation = (int16_t)(ReadingHeight - SetPointHeight); // positive when too high
tmp_int = (HeightDeviation * (int16_t)FCParam.HeightP) / 16; // p-part
HCGas -= tmp_int;
// ------------------------- D-Part 1: Vario Meter ----------------------------
tmp_int = ReadingVario / 8;
if(tmp_int > 8) tmp_int = 8; // limit quadratic part on upward movement to avoid to much gas reduction
if(tmp_int > 0) tmp_int = ReadingVario + (tmp_int * tmp_int) / 4;
else tmp_int = ReadingVario - (tmp_int * tmp_int) / 4;
tmp_int = (FCParam.HeightD * (int32_t)(tmp_int)) / 128L; // scale to d-gain parameter
LIMIT_MIN_MAX(tmp_int, -127, 255);
HCGas -= tmp_int;
// ------------------------ D-Part 2: ACC-Z Integral ------------------------
tmp_int = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN);
LIMIT_MIN_MAX(tmp_int, -127, 255);
HCGas -= tmp_int;
 
// limit deviation from hoover point within the target region
if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero
{
StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value
StickGasHoover = (StickGasHoover * UBat) / LowVoltageWarning;
LIMIT_MIN_MAX(StickGasHoover, 70, 150); // reserve some range for trim up and down
LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point
}
} // EOF trimming height set point
} //if MKFlags & MKFLAG_FLY
else // not flying but height control is already active
{
SetPointHeight = ReadingHeight - 400; // setpoint should be 4 meters below actual height to avoid a take off
if(ParamSet.Height_StickNeutralPoint) StickGasHoover = ParamSet.Height_StickNeutralPoint;
else StickGasHoover = RC_GAS_OFFSET;
}
} // EOF no baro range expanding
 
HCGas = HooverGas; // take hoover gas (neutral point for PD controller)
 
} //EOF alternative height control
 
if((ReadingHeight > SetPointHeight) || !(ParamSet.Config2 & CFG2_HEIGHT_LIMIT) )
{
// from this point the Heigth Control Algorithm is identical for both versions
if(BaroExpandActive) // baro range expanding active
{
HCGas = HooverGas; // hoover while expanding baro adc range
} // EOF // baro range expanding active
else // no baro range expanding
{
// ------------------------- P-Part ----------------------------
HeightDeviation = (int16_t)(ReadingHeight - SetPointHeight); // positive when too high
tmp_int = (HeightDeviation * (int16_t)FCParam.HeightP) / 16; // p-part
HCGas -= tmp_int;
// ------------------------- D-Part 1: Vario Meter ----------------------------
tmp_int = ReadingVario / 8;
if(tmp_int > 8) tmp_int = 8; // limit quadratic part on upward movement to avoid to much gas reduction
if(tmp_int > 0) tmp_int = ReadingVario + (tmp_int * tmp_int) / 4;
else tmp_int = ReadingVario - (tmp_int * tmp_int) / 4;
tmp_int = (FCParam.HeightD * (int32_t)(tmp_int)) / 128L; // scale to d-gain parameter
// ------------------------ D-Part 3: GpsZ ----------------------------------
tmp_int = (ParamSet.Height_GPS_Z * (int32_t)NCGpsZ)/128L;
LIMIT_MIN_MAX(tmp_int, -127, 255);
HCGas -= tmp_int;
// ------------------------ D-Part 2: ACC-Z Integral ------------------------
tmp_int = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN);
LIMIT_MIN_MAX(tmp_int, -127, 255);
HCGas -= tmp_int;
 
// limit deviation from hoover point within the target region
if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero
// strech control output by inverse attitude projection 1/cos
tmp_long2 = (int32_t)HCGas;
tmp_long2 *= 8192L;
tmp_long2 /= CosAttitude;
HCGas = (int16_t)tmp_long2;
 
// update height control gas averaging
FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE;
// limit height control gas pd-control output
LIMIT_MIN_MAX(FilterHCGas, ParamSet.HeightMinGas * STICK_GAIN, (ParamSet.GasMax - 20) * STICK_GAIN);
// limit gas to stick position for limiting height version
if(ParamSet.Config2 & CFG2_HEIGHT_LIMIT)
{
LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point
LIMIT_MAX(FilterHCGas, GasMixFraction);
}
} // EOF no baro range expanding
 
// ------------------------ D-Part 3: GpsZ ----------------------------------
tmp_int = (ParamSet.Height_GPS_Z * (int32_t)NCGpsZ)/128L;
LIMIT_MIN_MAX(tmp_int, -127, 255);
HCGas -= tmp_int;
 
// strech control output by inverse attitude projection 1/cos
tmp_long2 = (int32_t)HCGas;
tmp_long2 *= 8192L;
tmp_long2 /= CosAttitude;
HCGas = (int16_t)tmp_long2;
 
// update height control gas averaging
FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE;
// limit height control gas pd-control output
LIMIT_MIN_MAX(FilterHCGas, ParamSet.HeightMinGas * STICK_GAIN, (ParamSet.GasMax - 20) * STICK_GAIN);
// limit gas to stick position for limiting height version
if(ParamSet.Config2 & CFG2_HEIGHT_LIMIT)
{
LIMIT_MAX(FilterHCGas, GasMixFraction);
}
// set GasMixFraction to HeightControlGasFilter
GasMixFraction = FilterHCGas;
} // EOF if((ReadingHeight > SetPointHeight) || !(ParamSet.Config2 & CFG2_HEIGHT_LIMIT))
}// EOF height control active
else // HC not active
{
// update hoover gas stick value when HC is not active
if(ParamSet.Height_StickNeutralPoint)
// set GasMixFraction to HeightControlGasFilter
GasMixFraction = FilterHCGas;
} // EOF if((ReadingHeight > SetPointHeight) || !(ParamSet.Config2 & CFG2_HEIGHT_LIMIT))
}// EOF height control active
else // HC not active
{
StickGasHoover = ParamSet.Height_StickNeutralPoint;
}
else // take real hoover stick position
{
StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value
StickGasHoover = (StickGasHoover * UBat) / LowVoltageWarning;
}
LIMIT_MIN_MAX(StickGasHoover, 70, 150); // reserve some range for trim up and down
} // EOF HC not active
 
// ----------------- Hoover Gas Estimation --------------------------------
// Hoover gas estimation by averaging gas control output on small z-velocities
// this is done only if height contol option is selected in global config and aircraft is flying
if((MKFlags & MKFLAG_FLY) && !(MKFlags & MKFLAG_EMERGENCY_LANDING))
{
if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (uint32_t)(GasMixFraction); // init estimation
if(abs(ReadingVario) < 100) // only on small vertical speed
{
tmp_long2 = (int32_t)GasMixFraction; // take current thrust
tmp_long2 *= CosAttitude; // apply attitude projection
tmp_long2 /= 8192;
// average vertical projected thrust
if(ModelIsFlying < 2000) // the first 4 seconds
{ // reduce the time constant of averaging by factor of 8 to get much faster a stable value
HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/8L);
HooverGasFilter += 8L * tmp_long2;
}
else if(ModelIsFlying < 4000) // the first 8 seconds
{ // reduce the time constant of averaging by factor of 4 to get much faster a stable value
HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/4L);
HooverGasFilter += 4L * tmp_long2;
}
else if(ModelIsFlying < 8000) // the first 16 seconds
{ // reduce the time constant of averaging by factor of 2 to get much faster a stable value
HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/2L);
HooverGasFilter += 2L * tmp_long2;
}
else //later
// update hoover gas stick value when HC is not active
if(ParamSet.Height_StickNeutralPoint)
{
HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE;
HooverGasFilter += tmp_long2;
StickGasHoover = ParamSet.Height_StickNeutralPoint;
}
HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE);
if(ParamSet.Height_HooverBand)
else // take real hoover stick position
{
int16_t band;
band = HooverGas / ParamSet.Height_HooverBand; // the higher the parameter the smaller the range
HooverGasMin = HooverGas - band;
HooverGasMax = HooverGas + band;
StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value
StickGasHoover = (StickGasHoover * UBat) / LowVoltageWarning;
}
else
{ // no limit
HooverGasMin = 0;
HooverGasMax = 1023;
}
} //EOF only on small vertical speed
}// EOF ----------------- Hoover Gas Estimation --------------------------------
}// EOF ParamSet.Config0 & CFG0_HEIGHT_CONTROL
LIMIT_MIN_MAX(StickGasHoover, 70, 150); // reserve some range for trim up and down
FilterHCGas = GasMixFraction; // init filter for HCGas witch current gas mix fraction
} // EOF HC not active
 
// ----------------- Hoover Gas Estimation --------------------------------
// Hoover gas estimation by averaging gas control output on small z-velocities
// this is done only if height contol option is selected in global config and aircraft is flying
if((MKFlags & MKFLAG_FLY) && !(MKFlags & MKFLAG_EMERGENCY_LANDING))
{
if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (uint32_t)(GasMixFraction); // init estimation
if(abs(ReadingVario) < 100) // only on small vertical speed
{
tmp_long2 = (int32_t)GasMixFraction; // take current thrust
tmp_long2 *= CosAttitude; // apply attitude projection
tmp_long2 /= 8192;
// average vertical projected thrust
if(ModelIsFlying < 2000) // the first 4 seconds
{ // reduce the time constant of averaging by factor of 8 to get much faster a stable value
HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/8L);
HooverGasFilter += 8L * tmp_long2;
}
else if(ModelIsFlying < 4000) // the first 8 seconds
{ // reduce the time constant of averaging by factor of 4 to get much faster a stable value
HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/4L);
HooverGasFilter += 4L * tmp_long2;
}
else if(ModelIsFlying < 8000) // the first 16 seconds
{ // reduce the time constant of averaging by factor of 2 to get much faster a stable value
HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/2L);
HooverGasFilter += 2L * tmp_long2;
}
else //later
{
HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE;
HooverGasFilter += tmp_long2;
}
HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE);
if(ParamSet.Height_HooverBand)
{
int16_t band;
band = HooverGas / ParamSet.Height_HooverBand; // the higher the parameter the smaller the range
HooverGasMin = HooverGas - band;
HooverGasMax = HooverGas + band;
}
else
{ // no limit
HooverGasMin = 0;
HooverGasMax = 1023;
}
} //EOF only on small vertical speed
}// EOF ----------------- Hoover Gas Estimation --------------------------------
} // EOF if(LoadBalancer == 1)
else
{ // if no update in the height control block then take last value
GasMixFraction = FilterHCGas;
}
}// EOF ParamSet.Config0 & CFG0_AIRPRESS_SENSOR
 
// limit gas to parameter setting
LIMIT_MIN_MAX(GasMixFraction, (ParamSet.GasMin + 10) * STICK_GAIN, (ParamSet.GasMax - 20) * STICK_GAIN);
 
/beta/Code Redesign killagreg/main.c
333,9 → 333,9
{
UpdateMotor = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
 
J4HIGH;
//J4HIGH;
MotorControl();
J4LOW;
//J4LOW;
 
SendMotorData(); // the flight control code
RED_OFF;
/beta/Code Redesign killagreg/rc.c
232,8 → 232,8
RED_ON;
}
// demux sum signal for channels 5 to 7 to J3, J4, J5
//if(index == 5) J3HIGH; else J3LOW;
//if(index == 6) J4HIGH; else J4LOW;
if(index == 5) J3HIGH; else J3LOW;
if(index == 6) J4HIGH; else J4LOW;
if(CPUType != ATMEGA644P) // not used as TXD1
{
if(index == 7) J5HIGH; else J5LOW;
298,8 → 298,8
}
index++; // next channel
// demux sum signal for channels 5 to 7 to J3, J4, J5
//if(index == 5) J3HIGH; else J3LOW;
//if(index == 6) J4HIGH; else J4LOW;
if(index == 5) J3HIGH; else J3LOW;
if(index == 6) J4HIGH; else J4LOW;
if(CPUType != ATMEGA644P) // not used as TXD1
{
if(index == 7) J5HIGH; else J5LOW;