67,6 → 67,7 |
unsigned int NeutralAccX=0, NeutralAccY=0; |
int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
int NeutralAccZ = 0; |
signed char NeutralAccZfine = 0; |
unsigned char ControlHeading = 0;// in 2° |
long IntegralNick = 0,IntegralNick2 = 0; |
long IntegralRoll = 0,IntegralRoll2 = 0; |
211,13 → 212,11 |
DebugOut.Analog[30] = GPS_Nick; |
DebugOut.Analog[31] = GPS_Roll; |
if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe; |
|
DebugOut.Analog[16] = Variance; |
DebugOut.Analog[17] = VerticalVelocity; |
DebugOut.Analog[18] = HoehenWertF; |
DebugOut.Analog[25] = Parameter_Hoehe_P; |
DebugOut.Analog[26] = Parameter_Luftdruck_D; |
|
//DebugOut.Analog[16] = Variance; |
//DebugOut.Analog[17] = VarioMeter; |
//DebugOut.Analog[18] = HoehenWertF; |
//DebugOut.Analog[25] = Parameter_Hoehe_P; |
//DebugOut.Analog[26] = Parameter_Luftdruck_D; |
} |
|
|
272,16 → 271,17 |
|
//############################################################################ |
// Nullwerte ermitteln |
void SetNeutral(unsigned char AccAdjustment) |
unsigned char SetNeutral(unsigned char AccAdjustment) // retuns: "sucess" |
//############################################################################ |
{ |
unsigned char i; |
unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0; |
unsigned char i, sucess = 1; |
unsigned int gier_neutral = 0, nick_neutral = 0, roll_neutral = 0, acc_z_neutral = 0; |
VersionInfo.HardwareError[0] = 0; |
// HEF4017Reset_ON; |
NeutralAccX = 0; |
NeutralAccY = 0; |
NeutralAccZ = 0; |
NeutralAccZfine = 0; |
|
AdNeutralNick = 0; |
AdNeutralRoll = 0; |
308,10 → 308,12 |
gier_neutral += AdWertGier; |
nick_neutral += AdWertNick; |
roll_neutral += AdWertRoll; |
acc_z_neutral += Aktuell_az; |
} |
AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
AdNeutralNick = (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
AdNeutralRoll = (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
AdNeutralGier = (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
NeutralAccZ = (acc_z_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
|
StartNeutralRoll = AdNeutralRoll; |
StartNeutralNick = AdNeutralNick; |
320,8 → 322,6 |
{ |
NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
NeutralAccZ = Aktuell_az; |
|
// Save ACC neutral settings to eeprom |
SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccX); |
SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
332,17 → 332,15 |
// restore from eeprom |
NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK); |
NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL); |
NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP); |
// strange settings? |
if(((unsigned int) NeutralAccX > 2048) || ((unsigned int) NeutralAccY > 2048) || ((unsigned int) NeutralAccZ > 1024)) |
if(((unsigned int) NeutralAccX > 2048) || ((unsigned int) NeutralAccY > 2048)/* || ((unsigned int) NeutralAccZ > 1024)*/) |
{ |
printf("\n\rACC not calibrated!\r\n"); |
NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
NeutralAccZ = Aktuell_az; |
sucess = 0; |
} |
} |
|
MesswertNick = 0; |
MesswertRoll = 0; |
MesswertGier = 0; |
356,7 → 354,6 |
Mess_Integral_Gier = 0; |
StartLuftdruck = Luftdruck; |
VarioMeter = 0; |
VerticalVelocitySum = 0; |
SummenHoehe = 0; Mess_Integral_Hoch = 0; |
KompassSollWert = KompassValue; |
KompassSignalSchlecht = 100; |
387,6 → 384,20 |
NickServoValue = ((128 - 60) * 4 * 16); // neutral position = lower 1/4 |
} |
|
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
signed int tilt1, tilt2; |
tilt1 = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
tilt2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
tilt1 = (int16_t)ihypot(tilt1,tilt2); // tilt angle over all |
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(ACC_AltitudeControl) if((NeutralAccZ < 682 - 25) || (NeutralAccZ > 682 + 25)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; sucess = 0;}; |
|
#else |
NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP); |
#endif |
|
if((AdNeutralNick < 150 * 16) || (AdNeutralNick > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_NICK; }; |
if((AdNeutralRoll < 150 * 16) || (AdNeutralRoll > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_ROLL; }; |
if((AdNeutralGier < 150 * 2) || (AdNeutralGier > 850 * 2)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW; }; |
393,10 → 404,13 |
if((NeutralAccX < 300) || (NeutralAccX > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_NICK; }; |
if((NeutralAccY < 300) || (NeutralAccY > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_ROLL; }; |
if((NeutralAccZ < 512) || (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; }; |
if(VersionInfo.HardwareError[0]) sucess = 0; |
carefree_old = 70; |
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
LIBFC_HoTT_Clear(); |
ACC_AltitudeFusion(2); // initalisation |
#endif |
return(sucess); |
} |
|
|
863,13 → 877,13 |
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
} |
// ServoActive = 0; |
SetNeutral(0); |
CalibrationDone = 1; |
CalibrationDone = SetNeutral(0); |
ServoActive = 1; |
DDRD |=0x80; // enable J7 -> Servo signal |
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
if(abs(Aktuell_az - NeutralAccZ) > 5 && ACC_AltitudeControl) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; }; |
if(VersionInfo.HardwareError[0]) SpeakHoTT = SPEAK_ERR_SENSOR; |
else |
if(!CalibrationDone) SpeakHoTT = SPEAK_ERR_CALIBARTION; |
else SpeakHoTT = SPEAK_CALIBRATE; |
#endif |
Piep(GetActiveParamSet(),120); |
884,10 → 898,12 |
MotorenEin = 0; |
delay_neutral = 0; |
modell_fliegt = 0; |
SetNeutral(1); |
CalibrationDone = 1; |
CalibrationDone = SetNeutral(1); |
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
SpeakHoTT = SPEAK_CALIBRATE; |
if(VersionInfo.HardwareError[0]) SpeakHoTT = SPEAK_ERR_SENSOR; |
else |
if(!CalibrationDone) SpeakHoTT = SPEAK_ERR_CALIBARTION; |
else SpeakHoTT = SPEAK_CALIBRATE; |
#endif |
Piep(GetActiveParamSet(),120); |
} |
1530,18 → 1546,15 |
else // delay, because of expanding the Baro-Range |
{ |
// now clear the D-values |
VarioMeter = 0; |
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
if(ACC_AltitudeControl) SummenHoehe = HoehenWert * SA_FILTER; |
if(ACC_AltitudeControl) ACC_AltitudeFusion(1); // init |
else SummenHoehe = HoehenWert * SM_FILTER; |
#else |
SummenHoehe = HoehenWert * SM_FILTER; |
#endif |
|
VerticalVelocitySum = 0; |
VarioMeter = 0; |
BaroExpandActive--; |
} |
|
// if height control is activated by an rc channel |
if(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
{ // check if parameter is less than activation threshold |
1722,8 → 1735,8 |
} |
if(HoehenWertF > SollHoehe || !(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT)) |
{ |
if(!ACC_AltitudeControl) |
{ |
if(!ACC_AltitudeControl) |
{ |
// from this point the Heigth Control Algorithm is identical for both versions |
if(BaroExpandActive) // baro range expanding active |
{ |
1806,9 → 1819,9 |
GasMischanteil = FilterHCGas; |
} |
else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode |
} |
else |
{ |
} |
else // ACC-Altitude control |
{ |
// from this point the Heigth Control Algorithm is identical for both versions |
if(BaroExpandActive) // baro range expanding active |
{ |
1825,7 → 1838,7 |
LIMIT_MIN_MAX(tmp_long, -511 * STICK_GAIN, 512 * STICK_GAIN); // more than full range makes sense |
GasReduction = tmp_long; |
// ------------------------ D-Part: ACC-Z Integral ------------------------ |
tmp_long = VerticalVelocity + AdWertAccHoch * Parameter_Hoehe_ACC_Wirkung/256; |
tmp_long = VarioMeter + (AdWertAccHoch * Parameter_Hoehe_ACC_Wirkung)/256; |
// ------------------------- D-Part: Vario Meter ---------------------------- |
if(WaypointTrimming) { |
Variance = AltitudeSetpointTrimming * 8; |
1837,7 → 1850,6 |
LIMIT_MIN_MAX(tmp_long,-511 * STICK_GAIN, 512 * STICK_GAIN); |
GasReduction += tmp_long; |
} // EOF no baro range expanding |
DebugOut.Analog[19] = -GasReduction; |
HCGas -= GasReduction; |
LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limits gas around hover point |
// strech control output by inverse attitude projection 1/cos |
1856,8 → 1868,8 |
LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
GasMischanteil = FilterHCGas; |
} |
else GasMischanteil = FilterHCGas;// + (GasMischanteil - HoverGas) / 4; // Qopter: geändert |
} |
else GasMischanteil = FilterHCGas; |
} // end of ACC-Altitude control |
} |
}// EOF height control active |
else // HC not active |
1879,8 → 1891,8 |
// this is done only if height contol option is selected in global config and aircraft is flying |
if((FC_StatusFlags & FC_STATUS_FLY))// && !(FC_SatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
{ |
//if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(HoverGas); // Qopter: geändert |
//if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(HoverGas); // Qopter: geändert |
if(StartTrigger == 1) StartTrigger = 2; |
tmp_long2 = (int32_t)GasMischanteil; // take current thrust |
tmp_long2 *= CosAttitude; // apply attitude projection |
1897,8 → 1909,7 |
HoverGasFilter += 4L * tmp_long2; |
} |
else //later |
//if(abs(VerticalVelocity) < 50 && abs(HoehenWertF - SollHoehe) < 256) // Qopter: geändert, Anpassung nötig? |
if(abs(VarioMeter) < 100 && abs(HoehenWert - SollHoehe) < 256) // only on small vertical speed & difference is small (only descending) |
if(abs(VarioMeter) < 100 && abs(HoehenWertF - SollHoehe) < 256) // only on small vertical speed & difference is small (only descending) |
{ |
HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE; |
HoverGasFilter += tmp_long2; |