示例#1
0
 public void EnableHeadingHold()
 {
     HeadingHoldEnabled = true;
     RollHoldEnabled    = true;
     YawPIDController.Reset();
     RollPIDController.Reset();
 }
示例#2
0
        public override void Drive(FlightCtrlState s)
        {
            UpdatePID();

            //SpeedHold (set AccelerationTarget automatically to hold speed)
            if (SpeedHoldEnabled)
            {
                double spd = vesselState.speedSurface;
                cur_acc = (spd - _spd) / Time.fixedDeltaTime;
                _spd    = spd;
                RealAccelerationTarget = (SpeedTarget - spd) / 4;
                a_err = (RealAccelerationTarget - cur_acc);
                AccelerationPIDController.intAccum = MuUtils.Clamp(AccelerationPIDController.intAccum, -1 / AccKi, 1 / AccKi);
                double t_act = AccelerationPIDController.Compute(a_err);
                if (!double.IsNaN(t_act))
                {
                    core.thrust.targetThrottle = (float)MuUtils.Clamp(t_act, 0, 1);
                }
                else
                {
                    core.thrust.targetThrottle = 0.0f;
                    AccelerationPIDController.Reset();
                }
            }

            //AltitudeHold (set VertSpeed automatically to hold altitude)
            if (AltitudeHoldEnabled)
            {
                RealVertSpeedTarget = convertAltitudeToVerticalSpeed(AltitudeTarget - vesselState.altitudeASL);
                RealVertSpeedTarget = UtilMath.Clamp(RealVertSpeedTarget, -VertSpeedTarget, VertSpeedTarget);
            }
            else
            {
                RealVertSpeedTarget = VertSpeedTarget;
            }

            pitch_err = roll_err = yaw_err = 0;
            pitch_act = roll_act = yaw_act = 0;

            //VertSpeedHold
            if (VertSpeedHoldEnabled)
            {
                // NOTE: 60-to-1 rule:
                // deltaAltitude = 2 * PI * r * deltaPitch / 360
                // Vvertical = 2 * PI * TAS * deltaPitch / 360
                // deltaPitch = Vvertical / Vhorizontal * 180 / PI
                double deltaVertSpeed = RealVertSpeedTarget - vesselState.speedVertical;
                double adjustment     = 180 / vesselState.speedSurface * deltaVertSpeed / Math.PI;
                RealPitchTarget = vesselState.vesselPitch + adjustment;

                RealPitchTarget = UtilMath.Clamp(RealPitchTarget, -PitchDownLimit, PitchUpLimit);
                pitch_err       = MuUtils.ClampDegrees180(RealPitchTarget - vesselState.vesselPitch);

                PitchPIDController.intAccum = UtilMath.Clamp(PitchPIDController.intAccum, -100 / PitKi, 100 / PitKi);
                pitch_act = PitchPIDController.Compute(pitch_err) / 100;

                //Debug.Log (p_act);
                if (double.IsNaN(pitch_act))
                {
                    PitchPIDController.Reset();
                }
                else
                {
                    s.pitch = Mathf.Clamp((float)pitch_act, -1, 1);
                }
            }

            //HeadingHold
            double curFlightPath = vesselState.HeadingFromDirection(vesselState.forward);

            // NOTE: we can not use vesselState.vesselHeading here because it interpolates headings internally
            //       i.e. turning from 1° to 359° will end up as (1+359)/2 = 180°
            //       see class MovingAverage for more details
            curr_yaw = vesselState.currentHeading - curFlightPath;

            if (HeadingHoldEnabled)
            {
                double toturn = MuUtils.ClampDegrees180(HeadingTarget - curFlightPath);

                if (Math.Abs(toturn) < 0.2)
                {
                    // yaw for small adjustments
                    RealYawTarget  = MuUtils.Clamp(toturn * 2, -YawLimit, YawLimit);
                    RealRollTarget = 0;
                }
                else
                {
                    // roll for large adjustments
                    RealYawTarget  = 0;
                    RealRollTarget = MuUtils.Clamp(toturn * 2, -RollLimit, RollLimit);
                }
            }
            else
            {
                RealRollTarget = RollTarget;
                RealYawTarget  = 0;
            }

            if (RollHoldEnabled)
            {
                RealRollTarget = UtilMath.Clamp(RealRollTarget, -BankAngle, BankAngle);
                RealRollTarget = UtilMath.Clamp(RealRollTarget, -RollLimit, RollLimit);
                roll_err       = MuUtils.ClampDegrees180(RealRollTarget - -vesselState.currentRoll);

                RollPIDController.intAccum = MuUtils.Clamp(RollPIDController.intAccum, -100 / RolKi, 100 / RolKi);
                roll_act = RollPIDController.Compute(roll_err) / 100;

                if (double.IsNaN(roll_act))
                {
                    RollPIDController.Reset();
                }
                else
                {
                    s.roll = Mathf.Clamp((float)roll_act, -1, 1);
                }
            }

            if (HeadingHoldEnabled)
            {
                RealYawTarget = UtilMath.Clamp(RealYawTarget, -YawLimit, YawLimit);
                yaw_err       = MuUtils.ClampDegrees180(RealYawTarget - curr_yaw);

                YawPIDController.intAccum = MuUtils.Clamp(YawPIDController.intAccum, -100 / YawKi, 100 / YawKi);
                yaw_act = YawPIDController.Compute(yaw_err) / 100;

                if (double.IsNaN(yaw_act))
                {
                    YawPIDController.Reset();
                }
                else
                {
                    s.yaw = Mathf.Clamp((float)yaw_act, -1, 1);
                }
            }
        }