| 241,7 → 241,7 |
|---|
| BiasHiResGyroRoll = (int16_t)((Sum_2 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE); |
| AdBiasGyroYaw = (int16_t)((Sum_3 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE); |
| |
| if(AccAdjustment) |
| if(AccAdjustment != NO_ACC_CALIB) |
| { |
| // determine acc bias by averaging (require horizontal adjustment in nick and roll attitude) |
| #define ACC_BIAS_AVERAGE 10 |
| 1518,7 → 1518,7 |
|---|
| // if height control is activated |
| if(ParamSet.GlobalConfig & CFG_AIRPRESS_SENSOR) |
| { |
| #define HOOVER_GAS_AVERAGE 4192L // 4192 * 2ms = 8.2s averaging |
| #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
| #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging |
| |
| int16_t CosNick, CosRoll; |
| 1705,7 → 1705,7 |
|---|
| if((MKFlags & MKFLAG_FLY) && !(MKFlags & MKFLAG_EMERGENCY_LANDING)) |
| { |
| if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (uint32_t)(GasMixFraction); // init estimation |
| if(abs(ReadingVario) < 75) // only on small vertical speed |
| if(abs(ReadingVario) < 100) // only on small vertical speed |
| { |
| tmp_long3 = (int32_t)GasMixFraction; // take current thrust |
| tmp_long3 *= CosNick; // apply nick projection |
| 1714,10 → 1714,20 |
|---|
| tmp_long3 /= 8129; |
| // 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_long3; |
| } |
| 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_long3; |
| } |
| 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_long3; |
| } |
| else //later |
| { |
| HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE; |