| 648,7 → 648,7 |
|---|
| /************************************************************************/ |
| void MotorControl(void) |
| { |
| int16_t tmp_int, tmp_int2; |
| int16_t tmp_int1, tmp_int2; |
| int32_t tmp_long, tmp_long2; |
| |
| // Mixer Fractions that are combined for Motor Control |
| 668,15 → 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; |
| |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // RC-signal is bad |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1327,11 → 1323,11 |
|---|
| } |
| } |
| // exponential stick sensitivity in yawring rate |
| tmp_int = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx² |
| tmp_int += (ParamSet.StickYawP * StickYaw) / 4; |
| SetPointYaw = tmp_int; |
| tmp_int1 = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx² |
| tmp_int1 += (ParamSet.StickYawP * StickYaw) / 4; |
| SetPointYaw = tmp_int1; |
| // trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw) |
| ReadingIntegralGyroYaw -= tmp_int; |
| ReadingIntegralGyroYaw -= tmp_int1; |
| // limit the effect |
| LIMIT_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000) |
| |
| 1495,9 → 1491,9 |
|---|
| LIMIT_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT); |
| LIMIT_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Height Control |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Height Control |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| GasMixFraction = StickGas; // take the direct stick command |
| // at full LiPo the voltage is higher that gives more trust at the same BL-Control settpoint |
| // therefore attenuate the gas proportional to the lipo voltage reserve over the low bat warning level |
| 1508,324 → 1504,316 |
|---|
| } |
| GasMixFraction *= STICK_GAIN; // scale GasMixFraction to enlarge resolution in the motor mixer |
| |
| // if airpressure sensor is enabled |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Airpressure sensor is enabled |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if((ParamSet.Config0 & CFG0_AIRPRESS_SENSOR) && !(LoopingRoll || LoopingNick) ) |
| { |
| #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.1s averaging |
| #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging |
| |
| int16_t CosAttitude; // for projection of hoover gas |
| int16_t HCGas, HeightDeviation; |
| static int16_t FilterHCGas = 0; |
| 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; |
| |
| 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 |
| #define BARO_LIMIT_MAX 0x01 |
| #define BARO_LIMIT_MIN 0x02 |
| static uint8_t BaroFlags = 0; |
| static uint8_t BaroExpandActive = 0; |
| |
| 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; |
| // get the current hooverpoint |
| DebugOut.Analog[21] = HooverGas; |
| DebugOut.Analog[18] = ReadingVario; |
| |
| #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; |
| |
| // --------- 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 = 50; |
| } |
| else |
| { |
| BaroFlags |= BARO_LIMIT_MIN; |
| } |
| // --------- 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; |
| } |
| // 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); |
| { |
| BaroFlags |= BARO_LIMIT_MIN; |
| } |
| }// EOF --------- barometer range expansion ------------------ |
| |
| |
| |
| // 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; |
| } |
| } |
| // 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 = 250; |
| } |
| else |
| { //height control is activated |
| HCActive = 1; // enable height control |
| delay = 40; |
| { |
| BaroFlags |= BARO_LIMIT_MAX; |
| } |
| } |
| 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 |
| { // still ok |
| BaroFlags &= ~(BARO_LIMIT_MIN | BARO_LIMIT_MAX); |
| } |
| }// EOF --------- barometer range expansion ------------------ |
| |
| |
| // 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 = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain; |
| SetPointHeight = ReadingHeight; // update SetPoint with current reading |
| HCActive = 0; // disable height control |
| delay = 1; |
| } |
| HCActive = 1; |
| } |
| 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)) ) |
| { |
| SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain; |
| } |
| HCActive = 1; |
| } |
| |
| |
| // 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 |
| // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
| tmp_int1 = (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_int1, 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(HCActive && !(MKFlags & MKFLAG_EMERGENCY_LANDING)) |
| { |
| if((ParamSet.Config2 & CFG2_HEIGHT_LIMIT) || !(ParamSet.Config0 & CFG0_HEIGHT_SWITCH)) |
| { |
| 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. |
| // Holgers 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(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(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_DOWN; |
| SetPointHeight = ReadingHeight; // update setpoint to current height |
| } |
| else // gas stick in hoover range |
| 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) |
| { |
| 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_UP; |
| SetPointHeight = ReadingHeight; // update setpoint to current heigth |
| } |
| // trim height set point if needed |
| if(abs(HeightTrimming) > 256) |
| HeightTrimmingFlag |= HC_TRIM_DOWN; |
| HeightTrimming -= abs(StickGas - (StickGasHoover - HC_STICKTHRESHOLD)); |
| } |
| else // gas stick in hoover range |
| { |
| if(HeightTrimmingFlag & (HC_TRIM_UP|HC_TRIM_DOWN)) |
| { |
| SetPointHeight += (HeightTrimming * ParamSet.Height_Gain)/256; // move setpoint |
| HeightTrimmingFlag &= ~(HC_TRIM_UP|HC_TRIM_DOWN); |
| HeightTrimming = 0; |
| 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; // update setpoint to current height |
| if(ParamSet.Config2 & CFG2_VARIO_BEEP) BeepTime = 500; |
| } |
| } |
| // trim height set point if needed |
| if(abs(HeightTrimming) > 512) |
| { |
| 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; |
| } |
| SetPointHeight += (HeightTrimming * ParamSet.Height_Gain)/((5 * 512) / 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) |
| { |
| 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; |
| } |
| |
| HCGas = HooverGas; // take hoover gas (neutral point for PD controller) |
| HCGas = HooverGas; // take hoover gas (neutral point for PD controller) |
| |
| } //EOF alternative height control |
| } //EOF alternative height control |
| |
| if((ReadingHeight > SetPointHeight) || !(ParamSet.Config2 & CFG2_HEIGHT_LIMIT) ) |
| 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 |
| { |
| // 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_int1 = (HeightDeviation * (int16_t)FCParam.HeightP) / 16; // p-part |
| HCGas -= tmp_int1; |
| // ------------------------- D-Part 1: Vario Meter ---------------------------- |
| tmp_int1 = ReadingVario / 8; |
| if(tmp_int1 > 8) tmp_int1 = 8; // limit quadratic part on upward movement to avoid to much gas reduction |
| if(tmp_int1 > 0) tmp_int1 = ReadingVario + (tmp_int1 * tmp_int1) / 4; |
| else tmp_int1 = ReadingVario - (tmp_int1 * tmp_int1) / 4; |
| tmp_int1 = (FCParam.HeightD * (int32_t)(tmp_int1)) / 128L; // scale to d-gain parameter |
| LIMIT_MIN_MAX(tmp_int1, -127, 255); |
| HCGas -= tmp_int1; |
| // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
| tmp_int1 = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN); |
| LIMIT_MIN_MAX(tmp_int1, -127, 255); |
| HCGas -= tmp_int1; |
| |
| // 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 |
| { |
| 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_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
| } |
| } // EOF no baro range expanding |
| |
| // 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 |
| { |
| LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
| } |
| } // EOF no baro range expanding |
| // ------------------------ D-Part 3: GpsZ ---------------------------------- |
| tmp_int1 = (ParamSet.Height_GPS_Z * (int32_t)NCGpsZ)/128L; |
| LIMIT_MIN_MAX(tmp_int1, -127, 255); |
| HCGas -= tmp_int1; |
| |
| // ------------------------ 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; |
| |
| // 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) |
| { |
| 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 |
| FilterHCGas = GasMixFraction; // init filter for HCGas witch current gas mix fraction |
| } // EOF HC not active |
| |
| // 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 |
| // ----------------- 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 |
| { |
| // update hoover gas stick value when HC is not active |
| if(ParamSet.Height_StickNeutralPoint) |
| 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 |
| { |
| StickGasHoover = ParamSet.Height_StickNeutralPoint; |
| HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE; |
| HooverGasFilter += tmp_long2; |
| } |
| else // take real hoover stick position |
| HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE); |
| if(ParamSet.Height_HooverBand) |
| { |
| StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
| StickGasHoover = (StickGasHoover * UBat) / LowVoltageWarning; |
| int16_t band; |
| band = HooverGas / ParamSet.Height_HooverBand; // the higher the parameter the smaller the range |
| HooverGasMin = HooverGas - band; |
| HooverGasMax = HooverGas + band; |
| } |
| 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 |
| else |
| { // no limit |
| HooverGasMin = 0; |
| HooverGasMax = 1023; |
| } |
| } //EOF only on small vertical speed |
| }// EOF ----------------- Hoover Gas Estimation -------------------------------- |
| |
| // ----------------- 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 |
| 1861,8 → 1849,8 |
|---|
| { |
| LIMIT_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
| } |
| tmp_int = ParamSet.GasMax * STICK_GAIN; |
| LIMIT_MIN_MAX(YawMixFraction, -(tmp_int - GasMixFraction), (tmp_int - GasMixFraction)); |
| tmp_int1 = ParamSet.GasMax * STICK_GAIN; |
| LIMIT_MIN_MAX(YawMixFraction, -(tmp_int1 - GasMixFraction), (tmp_int1 - GasMixFraction)); |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Nick-Axis |
| 1885,9 → 1873,9 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Limiter |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction) / 2)) / 64; |
| LIMIT_MIN_MAX(NickMixFraction, -tmp_int, tmp_int); |
| LIMIT_MIN_MAX(RollMixFraction, -tmp_int, tmp_int); |
| tmp_int1 = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction) / 2)) / 64; |
| LIMIT_MIN_MAX(NickMixFraction, -tmp_int1, tmp_int1); |
| LIMIT_MIN_MAX(RollMixFraction, -tmp_int1, tmp_int1); |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Universal Mixer |