| 646,7 → 646,7 |
|---|
| void MotorControl(void) |
| { |
| int16_t tmp_int; |
| int32_t tmp_long3; |
| int32_t tmp_long, tmp_long2, tmp_long3; |
| |
| // Mixer Fractions that are combined for Motor Control |
| int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction; |
| 946,7 → 946,7 |
|---|
| |
| // mapping of gas |
| #define RC_GAS_BIAS 120 |
| StickGas = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + RC_GAS_BIAS;// shift to positive numbers |
| StickGas = (StickGas + PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + RC_GAS_BIAS) / 2;// shift to positive numbers |
| |
| // update gyro control loop factors |
| GyroPFactor = FCParam.GyroP + 10; |
| 1087,7 → 1087,6 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(! LoopingNick && !LoopingRoll && ( (AdValueAccZ > 512) || (MKFlags & MKFLAG_MOTOR_RUN) ) ) // if not lopping in any direction |
| { |
| int32_t tmp_long, tmp_long2; |
| if( FCParam.KalmanK != -1) |
| { |
| // determine the deviation of gyro integral from averaged acceleration sensor |
| 1522,7 → 1521,8 |
|---|
| #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
| #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging |
| |
| int16_t CosNick, CosRoll; |
| 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; |
| 1579,13 → 1579,16 |
|---|
| } |
| |
| // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
| CosNick = IntegralGyroNick/GYRO_DEG_FACTOR; // nick angle in deg |
| LIMIT_MIN_MAX(CosNick, -60, 60); // limit nick angle |
| CosNick = c_cos_8192(CosNick); |
| CosRoll = IntegralGyroRoll/GYRO_DEG_FACTOR; // roll angle in deg |
| LIMIT_MIN_MAX(CosRoll, -60, 60); // limit roll angle |
| CosRoll = c_cos_8192(CosRoll); |
| tmp_long = IntegralGyroNick/GYRO_DEG_FACTOR; // nick angle in deg |
| tmp_long *= tmp_long; |
| tmp_long2 = IntegralGyroRoll/GYRO_DEG_FACTOR; // roll angle in deg |
| tmp_long2 *= tmp_long2; |
| |
| CosAttitude = (int16_t)c_sqrt(tmp_long + tmp_long2); // 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 |
| DebugOut.Analog[17] = CosAttitude; |
| if(HCActive && !(MKFlags & MKFLAG_EMERGENCY_LANDING)) |
| { |
| if((ParamSet.Config2 & CFG2_HEIGHT_LIMIT) || !(ParamSet.Config0 & CFG0_HEIGHT_SWITCH)) |
| 1647,6 → 1650,9 |
|---|
| SetPointHeight += (HeightTrimming * ParamSet.Height_Gain)/(( 512 * 5 ) / 2); // move setpoint |
| HeightTrimming = 0; |
| if(ParamSet.Config2 & 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) / LowVoltageWarning; |
| } |
| } //if MKFlags & MKFLAG_FLY |
| else // not flying but height control is already active |
| 1692,12 → 1698,10 |
|---|
| DebugOut.Analog[23] = -tmp_int; |
| HCGas -= tmp_int; |
| |
| // strech control output by inverse attitude projection |
| // strech control output by inverse attitude projection 1/cos |
| tmp_long3 = (int32_t)HCGas; |
| tmp_long3 *= 8192; |
| tmp_long3 /= CosNick; |
| tmp_long3 *= 8192; |
| tmp_long3 /= CosRoll; |
| tmp_long3 *= 8192L; |
| tmp_long3 /= CosAttitude; |
| HCGas = (int16_t)tmp_long3; |
| |
| // update height control gas averaging |
| 1715,7 → 1719,7 |
|---|
| }// EOF height control active |
| else // HC not active |
| { |
| //update hoover gas stick value only if HC is not active |
| //update hoover gas stick value when HC is not active |
| StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
| StickGasHoover = (StickGasHoover * UBat) / LowVoltageWarning; |
| } |
| 1729,10 → 1733,8 |
|---|
| if(abs(ReadingVario) < 100) // only on small vertical speed |
| { |
| tmp_long3 = (int32_t)GasMixFraction; // take current thrust |
| tmp_long3 *= CosNick; // apply nick projection |
| tmp_long3 *= CosAttitude; // apply attitude projection |
| tmp_long3 /= 8192; |
| tmp_long3 *= CosRoll; // apply roll projection |
| 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 |