double compute_terminal_velocity() { double terminal_velocity = 0; if (Body.atmosphere && (VSL.VerticalSpeed.Absolute > -100 || VSL.Altitude.Relative < 100 + VSL.Geometry.H)) { terminal_velocity = Utils.ClampL(-VSL.VerticalSpeed.Absolute, 0.1f); VSL.Info.Countdown = (VSL.Altitude.Relative - VSL.Geometry.H) / terminal_velocity; } else { VSL.Info.Countdown = sim.FreeFallTime(out terminal_velocity); } return(terminal_velocity); }
protected override void Update() { SetState(TCAState.AltitudeControl); //calculate current altitude or apoapsis, if the vessel is moving upwards var alt = VSL.Altitude.Absolute; if (VSL.VerticalSpeed.Absolute > 0 && !VSL.LandedOrSplashed) { var ttAp = VSL.VerticalSpeed.Absolute / VSL.Physics.G; if (RAD != null && RAD.TimeAhead > 0 && RAD.TimeAhead < ttAp) { ttAp = RAD.TimeAhead; } alt = VSL.Altitude.Absolute + ttAp * (VSL.VerticalSpeed.Absolute - ttAp * VSL.Physics.G / 2); // CSV(CFG.DesiredAltitude, alt-VSL.Altitude.TerrainAltitude, VSL.Altitude.Relative, RAD.AltitudeAhead-VSL.Altitude.Absolute, // VSL.VSF, -VSL.Physics.G, ttAp, RAD.TimeAhead);//debug } //correct for terrain altitude and radar data if following terrain if (CFG.AltitudeAboveTerrain) { var obstacle_ahead = VSL.HorizontalSpeed.MoovingFast && alt - VSL.Altitude.Ahead <= VSL.Geometry.H; if (obstacle_ahead) { var dAlt = VSL.Altitude.Ahead + CFG.DesiredAltitude - VSL.Altitude.Absolute; SetState(VSL.VerticalSpeed.Absolute < 0? TCAState.GroundCollision : TCAState.ObstacleAhead); if (VSL.Altitude.CorrectionAllowed && RAD != null && RAD.TimeAhead > 0 && dAlt / RAD.TimeAhead > VerticalSpeedControl.C.MaxSpeed) { // var G_A = Utils.ClampL(VSL.Physics.G*(1-VSL.OnPlanetParams.DTWR), 1e-5f); // CFG.VerticalCutoff = Mathf.Sqrt(2*Utils.ClampL(dAlt * G_A, 0)); // ttAp = CFG.VerticalCutoff/G_A; // if(ttAp > RAD.TimeAhead) CFG.VerticalCutoff = dAlt / Utils.ClampL(RAD.TimeAhead - CollisionPreventionSystem.C.LookAheadTime, 1e-5f); if (VSL.Engines.Slow) { var dV = CFG.VerticalCutoff - VSL.VerticalSpeed.Absolute; dV *= dV < 0 ? VSL.Engines.DecelerationTime10 : VSL.Engines.AccelerationTime90; CFG.VerticalCutoff += dV; } // Log("VSF {}, vV {}, hV {}, G_A {}, dAlt {}, ttAp {}, TimeAhead {}", // CFG.VerticalCutoff, VSL.VerticalSpeed.Absolute, VSL.HorizontalSpeed.Absolute, G_A, dAlt, ttAp, RAD.TimeAhead);//debug return; } } var lower_threshold = Mathf.Max(VSL.Altitude.TerrainAltitude, VSL.Altitude.LowerThreshold); if (VSL.Altitude.CorrectionAllowed) { lower_threshold = Mathf.Max(VSL.Altitude.Ahead, lower_threshold); } alt -= lower_threshold; if (alt < 0) { set_ascending_state(); } } //calculate altitude error var error = (CFG.DesiredAltitude - alt); //turn off the engines if landed if (VSL.LandedOrSplashed && error < 0 && VSL.Altitude.Relative <= VSL.Geometry.R) { // Log("nAlt {}, alt {}, error {}, VSF {}", CFG.DesiredAltitude, alt, error, CFG.VerticalCutoff);//debug CFG.VerticalCutoff = -VerticalSpeedControl.C.MaxSpeed; return; } //calculate min/max speed var min_speed = -C.MaxSpeedLow; var max_speed = C.MaxSpeedLow; if (error < 0) { min_speed = Utils.Clamp(C.MaxSpeedLow * (error + C.MaxSpeedErrorF) / C.MaxSpeedErrorF, -C.MaxSpeedHigh, -C.MaxSpeedLow); if (VSL.VerticalSpeed.Absolute < 0) { double terminal_velocity; var free_fall = VSL.OnPlanet && VSL.Body.atmosphere? (float)sim.FreeFallTime(VSL.Altitude.Relative + error, out terminal_velocity) : (VSL.VerticalSpeed.Absolute + Mathf.Sqrt(VSL.VerticalSpeed.Absolute * VSL.VerticalSpeed.Absolute - 2 * VSL.Physics.G * error)) / VSL.Physics.G; var brake_time = -VSL.VerticalSpeed.Absolute / (VSL.OnPlanetParams.MaxTWR - 1) / VSL.Physics.G; if (brake_time < 0) { min_speed = 0; } else if (brake_time > free_fall / 100) { min_speed = Utils.Clamp(-C.MaxSpeedHigh * (1 - brake_time / free_fall), min_speed, Utils.ClampL(free_fall * VSL.Physics.G * (1 - VSL.OnPlanetParams.MaxTWR), -C.MaxSpeedLow)); } // Log("error {}, vV {}, free_fall {}, brake_time {}, min_speed {}", // -error, -VSL.VerticalSpeed.Absolute, free_fall, brake_time, min_speed);//debug } } else if (error > 0) { max_speed = alt <= VSL.Geometry.H? C.MaxSpeedHigh : Utils.Clamp(C.MaxSpeedLow * (error - C.MaxSpeedErrorF) / C.MaxSpeedErrorF, C.MaxSpeedLow, C.MaxSpeedHigh); } //update pid parameters and vertical speed setpoint if (VSL.Engines.Slow) { jets_pid.Min = min_speed; jets_pid.Max = max_speed; jets_pid.P = Utils.ClampH(C.JetsPID.P / VSL.OnPlanetParams.MaxTWR / C.TWRd * Mathf.Clamp(Mathf.Abs(1 / VSL.VerticalSpeed.Absolute) * C.ErrF, 1, VSL.OnPlanetParams.MaxTWR * C.TWRd), C.JetsPID.P); jets_pid.D = C.JetsPID.D; if (CFG.AltitudeAboveTerrain) { jets_pid.D /= Utils.ClampL(VSL.HorizontalSpeed, 1); } jets_pid.D *= error < 0? 1 + VSL.Engines.AccelerationTime90 * C.SlowCorrectionF : 1 + VSL.Engines.DecelerationTime10 * C.SlowCorrectionF; jets_pid.Update(error); CFG.VerticalCutoff = jets_pid.Action; // Log("nAlt {}, alt {}, error {}, hV {}, VSF {}\njPID: {}", // CFG.DesiredAltitude, alt, error, VSL.HorizontalSpeed, CFG.VerticalCutoff, jets_pid);//debug } else { rocket_pid.Min = min_speed; rocket_pid.Max = max_speed; rocket_pid.D = C.RocketPID.D; if (CFG.AltitudeAboveTerrain) { rocket_pid.D /= Utils.ClampL(VSL.HorizontalSpeed, 1); } rocket_pid.Update(error); CFG.VerticalCutoff = rocket_pid.Action; // Log("nAlt {}, alt {}, error {}, hV {}, VSF {}\nrPID: {}", // CFG.DesiredAltitude, alt, error, VSL.HorizontalSpeed, CFG.VerticalCutoff, rocket_pid);//debug } //correct for relative vertical speed //if following the terrain and flying below desired altitude if (CFG.AltitudeAboveTerrain) { var dV = 0f; if (error > 0) { dV = Utils.ClampL((VSL.VerticalSpeed.Absolute - VSL.VerticalSpeed.Relative) / Utils.ClampL(alt / C.RelAltitudeFactor, 1), 0); } CFG.VerticalCutoff += dV; //Loosing Altitude alert Falling.RunIf(() => SetState(TCAState.LoosingAltitude), VSL.VerticalSpeed.Relative < 0 && VSL.CFG.VerticalCutoff - VSL.VerticalSpeed.Absolute > 0 && VSL.Altitude < CFG.DesiredAltitude - VSL.VerticalSpeed.Relative * C.TimeAhead); // Log("error {0}, dV: {1}, VSP: {2}, min speed {3}, max speed {4}", // error, dV, CFG.VerticalCutoff, min_speed, max_speed);//debug // CSV(alt, VSL.vessel.altitude, VSL.Altitude.TerrainAltitude, VSL.Altitude, RAD.AltitudeAhead, error, // CFG.VerticalCutoff, VSL.VSF, VSL.MinVSF, VSL.VerticalSpeed, VSL.VerticalSpeed.Relative, // dV, min_speed, max_speed);//debug } }
protected override void Update() { //calculate current altitude or apoapsis, if the vessel is moving upwards var alt = VSL.Altitude.Absolute; if (VSL.VerticalSpeed.Absolute > 0 && !VSL.LandedOrSplashed) { var ttAp = VSL.VerticalSpeed.Absolute / VSL.Physics.G; if (RAD != null && RAD.TimeAhead > 0 && RAD.TimeAhead < ttAp) { ttAp = RAD.TimeAhead; } alt = VSL.Altitude.Absolute + ttAp * (VSL.VerticalSpeed.Absolute - ttAp * VSL.Physics.G / 2); } //correct for terrain altitude and radar data if following terrain if (CFG.AltitudeAboveTerrain) { var obstacle_ahead = VSL.HorizontalSpeed.MoovingFast && alt - VSL.Altitude.Ahead <= VSL.Geometry.H; if (obstacle_ahead) { SetState(TCAState.GroundCollision); var dAlt = VSL.Altitude.Ahead + CFG.DesiredAltitude - VSL.Altitude.Absolute; if (VSL.Altitude.CorrectionAllowed && RAD != null && RAD.TimeAhead > 0 && dAlt / RAD.TimeAhead > VerticalSpeedControl.C.MaxSpeed) { CFG.VerticalCutoff = dAlt / Utils.ClampL(RAD.TimeAhead - CollisionPreventionSystem.C.LookAheadTime, 1e-5f); if (VSL.Engines.Slow) { var dV = CFG.VerticalCutoff - VSL.VerticalSpeed.Absolute; dV *= dV < 0 ? VSL.Engines.DecelerationTime10 : VSL.Engines.AccelerationTime90; CFG.VerticalCutoff += dV; } if (CFG.VerticalCutoff > 0) { SetState(TCAState.Ascending); } return; } } var lower_threshold = Mathf.Max(VSL.Altitude.TerrainAltitude, VSL.Altitude.LowerThreshold); if (VSL.Altitude.CorrectionAllowed) { lower_threshold = Mathf.Max(VSL.Altitude.Ahead, lower_threshold); } alt -= lower_threshold; if (alt < VSL.Geometry.H && CFG.VerticalCutoff > 0) { SetState(TCAState.Ascending); } } //calculate altitude error var error = (CFG.DesiredAltitude - alt); //turn off the engines if landed if (VSL.LandedOrSplashed && error < 0 && VSL.Altitude.Relative <= VSL.Geometry.R) { CFG.VerticalCutoff = -VerticalSpeedControl.C.MaxSpeed; return; } //calculate min/max speed var min_speed = -C.MaxSpeedHigh; var max_speed = C.MaxSpeedHigh; if (error < 0) { min_speed = Utils.ClampH(error / C.MaxSpeedErrorF, -C.MaxSpeedLow); if (VSL.VerticalSpeed.Absolute < 0) { var free_fall = VSL.OnPlanet && VSL.Body.atmosphere ? (float)sim.FreeFallTime(VSL.Altitude.Relative + error, out _) : (VSL.VerticalSpeed.Absolute + Mathf.Sqrt(VSL.VerticalSpeed.Absolute * VSL.VerticalSpeed.Absolute - 2 * VSL.Physics.G * error)) / VSL.Physics.G; var brake_time = -VSL.VerticalSpeed.Absolute / (VSL.OnPlanetParams.MaxTWR - 1) / VSL.Physics.G; if (brake_time < 0) { min_speed = 0; } else if (brake_time > free_fall / 100) { min_speed = Utils.Clamp(-C.MaxSpeedHigh * (1 - brake_time / free_fall), min_speed, Utils.ClampL(free_fall * VSL.Physics.G * (1 - VSL.OnPlanetParams.MaxTWR), -C.MaxSpeedLow)); } } } else { max_speed = Utils.ClampL(error / C.MaxSpeedErrorF, C.MaxSpeedLow); } var aeroTorqueRatio = VSL.Torque.Slow ? VSL.OnPlanetParams.AeroTorqueRatio : VSL.OnPlanetParams.MaxAeroTorqueRatio; if (aeroTorqueRatio > 0) { var speedLimit = VSL.VerticalSpeed.Absolute * C.AeroTorqueRatioThreshold / aeroTorqueRatio; if (VSL.VerticalSpeed.Absolute > 0) { max_speed = Mathf.Min(max_speed, speedLimit); } else { min_speed = Mathf.Max(min_speed, speedLimit); } } //update pid parameters and vertical speed setpoint if (VSL.Engines.Slow) { jets_pid.Min = min_speed; jets_pid.Max = max_speed; jets_pid.P = Utils.ClampH( C.JetsPID.P / VSL.OnPlanetParams.MaxTWR / C.TWRd * Mathf.Clamp(Mathf.Abs(1 / VSL.VerticalSpeed.Absolute) * C.ErrF, 1, VSL.OnPlanetParams.MaxTWR * C.TWRd), C.JetsPID.P); jets_pid.D = C.JetsPID.D; if (CFG.AltitudeAboveTerrain) { jets_pid.D /= Utils.ClampL(VSL.HorizontalSpeed, 1); } jets_pid.D *= error < 0 ? 1 + VSL.Engines.AccelerationTime90 * C.SlowCorrectionF : 1 + VSL.Engines.DecelerationTime10 * C.SlowCorrectionF; jets_pid.Update(error); CFG.VerticalCutoff = jets_pid.Action; } else { rocket_pid.Min = min_speed; rocket_pid.Max = max_speed; rocket_pid.D = C.RocketPID.D; if (CFG.AltitudeAboveTerrain) { rocket_pid.D /= Utils.ClampL(VSL.HorizontalSpeed, 1); } rocket_pid.Update(error); CFG.VerticalCutoff = rocket_pid.Action; } //correct for relative vertical speed //if following the terrain and flying below desired altitude if (CFG.AltitudeAboveTerrain) { var dV = 0f; if (error > 0) { dV = Utils.ClampL((VSL.VerticalSpeed.Absolute - VSL.VerticalSpeed.Relative) / Utils.ClampL(alt / C.RelAltitudeFactor, 1), 0); } CFG.VerticalCutoff += dV; //Loosing Altitude alert if (VSL.VerticalSpeed.Relative < 0 && VSL.CFG.VerticalCutoff - VSL.VerticalSpeed.Absolute > 0 && VSL.Altitude < CFG.DesiredAltitude - VSL.VerticalSpeed.Relative * C.TimeAhead) { if (Falling.TimePassed) { SetState(TCAState.LoosingAltitude); } } else { Falling.Reset(); } // Log("error {0}, dV: {1}, VSP: {2}, min speed {3}, max speed {4}", // error, dV, CFG.VerticalCutoff, min_speed, max_speed);//debug // CSV(alt, VSL.vessel.altitude, VSL.Altitude.TerrainAltitude, VSL.Altitude, RAD.AltitudeAhead, error, // CFG.VerticalCutoff, VSL.VSF, VSL.MinVSF, VSL.VerticalSpeed, VSL.VerticalSpeed.Relative, // dV, min_speed, max_speed);//debug } // DebugWindowController.PostMessage($"ALT-{GetHashCode():X8}", // $"AeroTorqueRatio: {aeroTorqueRatio:F6}\n" // + $"min speed: {min_speed:F1} m/s\n" // + $"max speed: {max_speed:F1} m/s\n" // + $"setpoint: {VSL.CFG.VerticalCutoff:F1} m/s\n" // + $"error abs: {VSL.CFG.VerticalCutoff - VSL.VerticalSpeed.Absolute:F1}"); }