95,7 → 95,8 |
char GyroFaktor,GyroFaktorGier; |
char IntegralFaktor,IntegralFaktorGier; |
int DiffNick,DiffRoll; |
int StickGasHover = 127, HoverGasMin = 0, HoverGasMax = 1023; |
unsigned int StickGasHover = 127; |
int HoverGasMin = 0, HoverGasMax = 1023; |
int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
//int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; |
unsigned char Poti[9] = {0,0,0,0,0,0,0,0}; |
412,7 → 413,7 |
CosAttitude = c_cos_8192(tilt1); |
NeutralAccZ = (long)((long) (NeutralAccZ - 512) * 8192 + 4096) / CosAttitude + 512; |
if(tilt1 > 20) sucess = 0; // calibration must be within 20° Tilt angle |
if(AdjustmentMode != 0 && ACC_AltitudeControl) if((NeutralAccZ < 682 - 25) || (NeutralAccZ > 682 + 35)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; sucess = 0;}; |
if(AdjustmentMode != 0 && ACC_AltitudeControl) if((NeutralAccZ < 682 - 30) || (NeutralAccZ > 682 + 35)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; sucess = 0;}; |
#else |
NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP); |
EEAR = EE_DUMMY; // Set the EEPROM Address pointer to an unused space |
615,22 → 616,23 |
// ++++++++++++++++++++++++++++++++++++++++++ |
|
// ++++++++++++++++++++++++++++++++++++++++++ |
#ifdef REDUNDANT_FC |
#ifdef REDUNDANT_FC_SLAVE |
I2CTimeout = 100; |
return; |
#else |
/* |
if(Parameter_UserParam7 < 100 && RedundanceBlOperation) |
#endif |
|
#ifdef REDUNDANT_FC_MASTER |
if(Parameter_UserParam7 > 150) |
{ |
// Make noise |
if((BeepMuster == 0xffff)) { beeptime = 25000; BeepMuster = 0x0080; } |
// Do not send I2C-Data |
if(RedundanceBlOperation) |
{ |
I2CTimeout = 100; |
if((BeepMuster == 0xffff)) |
{ |
beeptime = 25000; |
BeepMuster = 0x0080; |
} |
return; |
} |
*/ |
} |
#endif |
|
if(I2C_TransferActive) |
763,6 → 765,18 |
if(CareFree) { FC_StatusFlags2 |= FC_STATUS2_CAREFREE; /*if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;*/} else FC_StatusFlags2 &= ~FC_STATUS2_CAREFREE; |
} |
|
void CalcStickGasHover(void) |
{ |
if(!EE_Parameter.Hoehe_StickNeutralPoint) |
{ |
StickGasHover = HoverGas/STICK_GAIN; // rescale back to stick value |
StickGasHover = (unsigned int)((unsigned int) StickGasHover * UBat) / BattLowVoltageWarning; |
} |
else StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint; |
LIMIT_MIN_MAX(StickGasHover, 70, 175); // reserve some range for trim up and down |
} |
|
|
//############################################################################ |
// |
void MotorRegler(void) |
1623,7 → 1637,6 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Auto-Landing |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
static unsigned char slower; |
if(!slower--) |
{ |
1649,15 → 1662,13 |
FromNC_AltitudeSpeed = EE_Parameter.LandingSpeed; |
FromNC_AltitudeSetpoint = -20000; |
} |
//#endif |
//DebugOut.Analog[16] = StickGasMiddle; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// send SPI pending bytes |
if(BytegapSPI == 0) SPI_TransmitByte(); |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Höhenregelung |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMischanteil *= STICK_GAIN; |
if(BytegapSPI == 0) SPI_TransmitByte(); |
// if height control is activated |
if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick) && !(VersionInfo.HardwareError[0] & 0x7F)) // Höhenregelung |
{ |
1887,13 → 1898,16 |
LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied |
if(Parameter_ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100; |
//update hoover gas stick value when setpoint is shifted |
if(FromNC_AltitudeSpeed == 0) CalcStickGasHover(); |
/* |
if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0) |
{ |
StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value |
StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning; |
StickGasHover = (unsigned int)((unsigned int) StickGasHover * UBat) / BattLowVoltageWarning; |
if(StickGasHover < 70) StickGasHover = 70; |
else if(StickGasHover > 150) StickGasHover = 150; |
else if(StickGasHover > 175) StickGasHover = 175; |
} |
*/ |
} |
if(BaroExpandActive) SollHoehe = HoehenWertF; // update setpoint to current altitude if Expanding is active |
} //if FCFlags & MKFCFLAG_FLY |
1901,7 → 1915,7 |
{ |
SollHoehe = HoehenWert - 400; |
if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint; |
else StickGasHover = 120; |
else StickGasHover = 127; |
HoverGas = GasMischanteil; |
VarioCharacter = '.'; |
} |
2051,13 → 2065,15 |
else // HC not active |
{ |
//update hoover gas stick value when HC is not active |
if(!EE_Parameter.Hoehe_StickNeutralPoint) |
CalcStickGasHover(); |
/* if(!EE_Parameter.Hoehe_StickNeutralPoint) |
{ |
StickGasHover = HoverGas/STICK_GAIN; // rescale back to stick value |
StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning; |
StickGasHover = (unsigned int)((unsigned int) StickGasHover * UBat) / BattLowVoltageWarning; |
} |
else StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint; |
LIMIT_MIN_MAX(StickGasHover, 70, 150); // reserve some range for trim up and down |
LIMIT_MIN_MAX(StickGasHover, 70, 175); // reserve some range for trim up and down |
*/ |
FilterHCGas = GasMischanteil; |
// set both flags to indicate no vario mode |
FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
2142,6 → 2158,8 |
#endif |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// send SPI pending bytes |
if(BytegapSPI == 0) SPI_TransmitByte(); |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Mischer und PI-Regler |
2200,8 → 2218,9 |
if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// send SPI pending bytes |
if(BytegapSPI == 0) SPI_TransmitByte(); |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Universal Mixer |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2224,7 → 2243,7 |
if(Mixer.Motor[i][3] == 64) tmp_int += GierMischanteil; |
else if(Mixer.Motor[i][3] == -64) tmp_int -= GierMischanteil; |
else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
#ifdef REDUNDANT_FC |
#ifdef REDUNDANT_FC_SLAVE |
tmp_int = (tmp_motorwert[i] + tmp_int) / 2; |
#else |
if(Motor[i].Version & MOTOR_STATE_FAST_MODE || tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // Beschleunigen |
2253,6 → 2272,8 |
Motor[i].SetPointLowerBits = 0; |
} |
} |
//if(Parameter_UserParam6 > 200) Motor[0].SetPoint = 0; |
#ifdef REDUNDANT_FC_MASTER |
if(Parameter_UserParam6 > 200 && RedundanceBlOperation) Motor[0].SetPoint = 0; |
#endif |
} |
//DebugOut.Analog[16] |