56,6 → 56,7 |
#include "main.h" |
#include "eeprom.c" |
#include "mymath.h" |
#include "isqrt.h" |
|
unsigned char h,m,s,BaroExpandActive = 0; |
volatile unsigned int I2CTimeout = 100; |
542,7 → 543,7 |
void MotorRegler(void) |
//############################################################################ |
{ |
int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int; |
int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2; |
int GierMischanteil,GasMischanteil; |
static long SummeNick=0,SummeRoll=0; |
static long sollGier = 0,tmp_long,tmp_long2; |
564,7 → 565,7 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMischanteil = StickGas; |
if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10; |
|
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Empfang schlecht |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
650,7 → 651,7 |
{ |
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
} |
ServoActive = 0; |
ServoActive = 0; |
SetNeutral(); |
ServoActive = 1; |
DDRD |=0x80; // enable J7 -> Servo signal |
746,7 → 747,7 |
if(StickGier < -2) StickGier += 2; else StickGier = 0; |
|
StickGas = (StickGas + PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120) / 2; |
|
|
GyroFaktor = (Parameter_Gyro_P + 10.0); |
IntegralFaktor = Parameter_Gyro_I; |
GyroFaktorGier = (Parameter_Gyro_Gier_P + 10.0); |
764,10 → 765,10 |
if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
} |
if(StickGas < 0) StickGas = 0; |
|
|
if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
if(GyroFaktor < 0) GyroFaktor = 0; |
if(IntegralFaktor < 0) IntegralFaktor = 0; |
//if(GyroFaktor < 0) GyroFaktor = 0; |
//if(IntegralFaktor < 0) IntegralFaktor = 0; |
|
if(abs(StickNick/STICK_GAIN) > MaxStickNick) |
{ |
1234,8 → 1235,7 |
static unsigned long HooverGasFilter = 0; |
static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
int CosAttitude; // for projection of hoover gas |
long tmp_long3; |
// const unsigned char GAS_TAB[31] = {128,128,128,129,129,130,131,132,133,135,136,138,140,142,145,148,151,154,158,162,167,172,178,184,191,199,208,218,229,241,255}; |
|
// get the current hooverpoint |
if(LoadHandler == 1) |
{ |
1248,35 → 1248,35 |
{ |
if(MessLuftdruck > 920) |
{ // increase offset |
if(OCR0A < (255 - OPA_OFFSET_STEP)) |
if(OCR0A < (255 - OPA_OFFSET_STEP)) |
{ |
ExpandBaro -= 1; |
OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
beeptime = 300; |
BaroExpandActive = 50; |
} |
else |
} |
else |
{ |
BaroAtLowerLimit = 1; |
} |
} |
// measurement of air pressure close to lower limit and |
else |
else |
if(MessLuftdruck < 100) |
{ // decrease offset |
if(OCR0A > OPA_OFFSET_STEP) |
if(OCR0A > OPA_OFFSET_STEP) |
{ |
ExpandBaro += 1; |
OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
beeptime = 300; |
BaroExpandActive = 50; |
} |
else |
} |
else |
{ |
BaroAtUpperLimit = 1; |
} |
} |
else |
else |
{ |
BaroAtUpperLimit = 0; |
BaroAtLowerLimit = 0; |
1315,12 → 1315,10 |
} |
|
// calculate cos of nick and roll angle used for projection of the vertical hoover gas |
J4Low; |
tmp_long = IntegralNick/GIER_GRAD_FAKTOR; // nick angle in deg |
tmp_long *= tmp_long; |
tmp_long2 = IntegralRoll/GIER_GRAD_FAKTOR; // roll angle in deg |
tmp_long2 *= tmp_long2; |
CosAttitude = (int16_t)c_sqrt(tmp_long + tmp_long2); // phytagoras gives effective attitude angle in deg |
J4Low; |
tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg |
DebugOut.Analog[16] = CosAttitude; |
LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
1331,7 → 1329,7 |
#define HEIGHT_TRIM_DOWN 0x02 |
static unsigned char HeightTrimmingFlag = 0x00; |
|
#define HEIGHT_CONTROL_STICKTHRESHOLD 15 |
#define HEIGHT_CONTROL_STICKTHRESHOLD 15 |
// Holger original version |
// start of height control algorithm |
// the height control is only an attenuation of the actual gas stick. |
1388,13 → 1386,13 |
if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75; |
//update hoover gas stick value when setpoint is shifted |
// StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
// StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
// if(StickGasHoover < 70) StickGasHoover = 70; |
// else if(StickGasHoover > 150) StickGasHoover = 150; |
// StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
// if(StickGasHoover < 70) StickGasHoover = 70; |
// else if(StickGasHoover > 150) StickGasHoover = 150; |
} |
} //if MikroKopterFlags & MKFLAG_FLY |
else |
{ |
else |
{ |
SollHoehe = HoehenWert - 400; |
if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
else StickGasHoover = 120; |
1420,7 → 1418,7 |
tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (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 |
{ |
1432,13 → 1430,13 |
tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L; |
LIMIT_MIN_MAX(tmp_int, -127, 255); |
HCGas -= tmp_int; |
|
|
// strech control output by inverse attitude projection 1/cos |
// + 1/cos(angle) ++++++++++++++++++++++++++ |
tmp_long3 = (int32_t)HCGas; |
tmp_long3 *= 8192L; |
tmp_long3 /= CosAttitude; |
HCGas = (int16_t)tmp_long3; |
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 |
1460,10 → 1458,10 |
StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
} |
else StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
if(StickGasHoover < 70) StickGasHoover = 70; |
else if(StickGasHoover > 150) StickGasHoover = 150; |
if(StickGasHoover < 70) StickGasHoover = 70; |
else if(StickGasHoover > 150) StickGasHoover = 150; |
FilterHCGas = GasMischanteil; |
} |
} |
|
// 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 |
1472,30 → 1470,30 |
if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
if(abs(VarioMeter) < 100) // only on small vertical speed |
{ |
tmp_long3 = (int32_t)GasMischanteil; // take current thrust |
tmp_long3 *= CosAttitude; // apply attitude projection |
tmp_long3 /= 8192; |
|
tmp_long2 = (int32_t)GasMischanteil; // take current thrust |
tmp_long2 *= CosAttitude; // apply attitude projection |
tmp_long2 /= 8192; |
|
// average vertical projected thrust |
if(modell_fliegt < 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_long3; |
HooverGasFilter += 8L * tmp_long2; |
} |
else if(modell_fliegt < 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_long3; |
HooverGasFilter += 4L * tmp_long2; |
} |
else if(modell_fliegt < 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_long3; |
HooverGasFilter += 2L * tmp_long2; |
} |
else //later |
{ |
HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE; |
HooverGasFilter += tmp_long3; |
HooverGasFilter += tmp_long2; |
} |
HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE); |
if(EE_Parameter.Hoehe_HoverBand) |
1513,11 → 1511,11 |
} |
} |
LastHCGas = GasMischanteil; |
} |
else |
} |
else |
{ |
GasMischanteil = LastHCGas; |
} |
} |
//DebugOut.Analog[26] = HooverGasMax; |
}// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
J4Low; |
1529,7 → 1527,7 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// all BL-Ctrl connected? |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(MissingMotor) |
if(MissingMotor) |
if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0) |
{ |
modell_fliegt = 1; |