Skip to content

Commit

Permalink
core: add etcs svl logic
Browse files Browse the repository at this point in the history 
Signed-off-by: Erashin <[email protected]>
  • Loading branch information
@Erashin
Erashin committed Feb 21, 2025
1 parent d1389d4 commit 6212f99
Show file tree
Hide file tree
Showing 2 changed files with 154 additions and 82 deletions.
3 changes: 3 additions & 0 deletions core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/etcs/Constants.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,9 @@ const val qNvinhsmicperm = false
*/
const val aEst2 = 0.4

// TODO: complete subset
const val releaseSpeed = 40.0 / 3.6 // m/s

/** See Subset 026: table in Appendix A.3.1. */
const val dvEbiMin = 7.5 / 3.6 // m/s
const val dvEbiMax = 15.0 / 3.6 // m/s
Expand Down
233 changes: 151 additions & 82 deletions core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/etcs/ETCSBrakingCurves.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,57 +47,87 @@ fun addBrakingCurvesAtEOAs(
): Envelope {
val sortedEndsOfAuthority = endsOfAuthority.sortedBy { it.offsetEOA }
var beginPos = envelope.beginPos
val builder = OverlayEnvelopeBuilder.forward(envelope)
val maxSpeedEnvelope = envelope.maxSpeed
var envelopeWithEoaBrakingCurves = envelope
var builder = OverlayEnvelopeBuilder.forward(envelopeWithEoaBrakingCurves)
for (endOfAuthority in sortedEndsOfAuthority) {
val targetPosition = endOfAuthority.offsetEOA.distance.meters
assert(targetPosition > 0.0)
val targetSpeed = 0.0
val maxSpeedEnvelope = envelope.maxSpeed
val overhead =
Envelope.make(
EnvelopePart.generateTimes(
listOf(EnvelopeProfile.CONSTANT_SPEED),
doubleArrayOf(0.0, targetPosition),
doubleArrayOf(maxSpeedEnvelope, maxSpeedEnvelope)
val fullIndicationCurveEoa =
computeSbdFullIndicationCurve(context, targetPosition, maxSpeedEnvelope)

if (endOfAuthority.offsetSVL != null) {
val targetPositionSvl = endOfAuthority.offsetSVL.distance.meters
val fullIndicationCurveSvl =
computeEbdFullIndicationCurve(
context,
targetPositionSvl,
targetSpeed,
maxSpeedEnvelope
)
val fullIndCurveSvlWithMaintain: Envelope
val releaseSpeedPosition = fullIndicationCurveSvl.interpolatePosition(releaseSpeed)
if (releaseSpeedPosition >= targetPosition) {
fullIndCurveSvlWithMaintain =
Envelope.make(
fullIndicationCurveSvl.slice(
fullIndicationCurveSvl.beginPos,
targetPosition
)
)
} else {
val maintainReleaseSpeedCurve =
EnvelopePart.generateTimes(
listOf(EnvelopeProfile.CONSTANT_SPEED),
doubleArrayOf(releaseSpeedPosition, targetPosition),
doubleArrayOf(releaseSpeed, releaseSpeed)
)
fullIndCurveSvlWithMaintain =
if (releaseSpeedPosition == fullIndicationCurveSvl.beginPos) {
Envelope.make(maintainReleaseSpeedCurve)
} else {
Envelope.make(
fullIndicationCurveEoa.sliceWithSpeeds(
fullIndicationCurveSvl.beginPos,
fullIndicationCurveSvl.beginSpeed,
releaseSpeedPosition,
releaseSpeed
),
maintainReleaseSpeedCurve
)
}
}
val indicationCurveSvl =
keepBrakingCurveUnderOverlay(fullIndCurveSvlWithMaintain, envelope, beginPos)
?: continue
assert(
indicationCurveSvl.beginPos >= beginPos &&
indicationCurveSvl.endPos == targetPosition
)
val sbdCurve =
computeBrakingCurve(
context,
overhead,
targetPosition,
targetSpeed,
BrakingType.ETCS_SBD
)
assert(sbdCurve.beginPos >= 0 && sbdCurve.endPos == targetPosition)
assert(sbdCurve.endSpeed == targetSpeed)
val guiCurve =
computeBrakingCurve(
context,
overhead,
targetPosition,
targetSpeed,
BrakingType.ETCS_GUI
assert(
indicationCurveSvl.beginSpeed <= maxSpeedEnvelope &&
indicationCurveSvl.endSpeed >= releaseSpeed
)
assert(guiCurve.beginPos >= 0.0 && guiCurve.endPos == targetPosition)
assert((guiCurve.beginSpeed == maxSpeedEnvelope || guiCurve.beginPos == 0.0))
assert(guiCurve.endSpeed == targetSpeed)

val fullIndicationCurve =
computeIndicationBrakingCurveFromRef(context, sbdCurve, BrakingCurveType.SBD, guiCurve)
assert(fullIndicationCurve.endPos == targetPosition)
assert(fullIndicationCurve.endSpeed == targetSpeed)
builder.addPart(indicationCurveSvl)
envelopeWithEoaBrakingCurves = builder.build()
builder = OverlayEnvelopeBuilder.forward(envelopeWithEoaBrakingCurves)
}

val indicationCurve =
keepBrakingCurveUnderOverlay(Envelope.make(fullIndicationCurve), envelope, beginPos)
?: continue
keepBrakingCurveUnderOverlay(
Envelope.make(fullIndicationCurveEoa),
envelopeWithEoaBrakingCurves,
beginPos
) ?: continue
assert(indicationCurve.beginPos >= beginPos && indicationCurve.endPos == targetPosition)
assert(
indicationCurve.beginSpeed <= maxSpeedEnvelope &&
indicationCurve.endSpeed == targetSpeed
)

builder.addPart(indicationCurve)
envelopeWithEoaBrakingCurves = builder.build()
builder = OverlayEnvelopeBuilder.forward(envelopeWithEoaBrakingCurves)

// We build EOAs along the path. We need to handle overlaps with the next EOA. To do so, we
// shift the left position constraint, beginPos, to this EOA's target position.
Expand All @@ -114,58 +144,19 @@ fun addBrakingCurvesAtLOAs(
): Envelope {
val sortedLimitsOfAuthority = limitsOfAuthority.sortedBy { it.offset }
val beginPos = envelope.beginPos
val maxSpeedEnvelope = envelope.maxSpeed
var envelopeWithLoaBrakingCurves = envelope
var builder = OverlayEnvelopeBuilder.forward(envelopeWithLoaBrakingCurves)

val maxSpeedEnvelope = envelopeWithLoaBrakingCurves.maxSpeed
// Add maxBecDeltaSpeed to EBD curve overhead so it reaches a sufficiently high speed to
// guarantee that, after the speed translation, the corresponding EBI curve does intersect
// with envelope max speed.
val maxBecDeltaSpeed = maxBecDeltaSpeed()
val maxSpeedEbd = maxSpeedEnvelope + maxBecDeltaSpeed
val overhead =
Envelope.make(
EnvelopePart.generateTimes(
listOf(EnvelopeProfile.CONSTANT_SPEED),
doubleArrayOf(0.0, context.path.length),
doubleArrayOf(maxSpeedEbd, maxSpeedEbd)
)
)

for (limitOfAuthority in sortedLimitsOfAuthority) {
val targetPosition = limitOfAuthority.offset.distance.meters
assert(targetPosition > 0.0)
val targetSpeed = limitOfAuthority.speed
assert(targetSpeed > 0.0)

val ebdCurve =
computeBrakingCurve(
context,
overhead,
targetPosition,
targetSpeed,
BrakingType.ETCS_EBD
)
assert(ebdCurve.beginPos >= 0.0 && ebdCurve.endPos >= targetPosition)
val guiCurve =
computeBrakingCurve(
context,
overhead,
targetPosition,
targetSpeed,
BrakingType.ETCS_GUI
)
assert(guiCurve.beginPos >= 0.0 && guiCurve.endPos == targetPosition)
assert((guiCurve.beginSpeed == maxSpeedEbd || guiCurve.beginPos == 0.0))

val ebiCurve = computeEbiBrakingCurveFromEbd(context, ebdCurve, targetSpeed)
assert(ebiCurve.endSpeed == targetSpeed)

val fullIndicationCurve =
computeIndicationBrakingCurveFromRef(context, ebiCurve, BrakingCurveType.EBI, guiCurve)
computeEbdFullIndicationCurve(context, targetPosition, targetSpeed, maxSpeedEnvelope)
val endOfIndicationCurve = fullIndicationCurve.endPos
assert(endOfIndicationCurve <= targetPosition)
assert(fullIndicationCurve.endSpeed == targetSpeed)

val fullIndCurveWithMaintain: Envelope
if (endOfIndicationCurve < targetPosition) {
Expand Down Expand Up @@ -203,6 +194,79 @@ fun addBrakingCurvesAtLOAs(
return envelopeWithLoaBrakingCurves
}

/** Compute SBD-based full indication curve. */
private fun computeSbdFullIndicationCurve(
context: EnvelopeSimContext,
targetPosition: Double,
maxSpeedEnvelope: Double
): EnvelopePart {
val targetSpeed = 0.0
val overhead =
Envelope.make(
EnvelopePart.generateTimes(
listOf(EnvelopeProfile.CONSTANT_SPEED),
doubleArrayOf(0.0, targetPosition),
doubleArrayOf(maxSpeedEnvelope, maxSpeedEnvelope)
)
)
val sbdCurve =
computeBrakingCurve(context, overhead, targetPosition, targetSpeed, BrakingType.ETCS_SBD)
assert(sbdCurve.beginPos >= 0 && sbdCurve.endPos == targetPosition)
assert(sbdCurve.endSpeed == targetSpeed)

val guiCurve =
computeBrakingCurve(context, overhead, targetPosition, targetSpeed, BrakingType.ETCS_GUI)
assert(guiCurve.beginPos >= 0.0 && guiCurve.endPos == targetPosition)
assert((guiCurve.beginSpeed == maxSpeedEnvelope || guiCurve.beginPos == 0.0))
assert(guiCurve.endSpeed == targetSpeed)

val fullIndicationCurve =
computeIndicationBrakingCurveFromRef(context, sbdCurve, BrakingCurveType.SBD, guiCurve)
assert(fullIndicationCurve.endPos == targetPosition)
assert(fullIndicationCurve.endSpeed == targetSpeed)
return fullIndicationCurve
}

/** Compute EBD-based full indication curve. */
private fun computeEbdFullIndicationCurve(
context: EnvelopeSimContext,
targetPosition: Double,
targetSpeed: Double,
maxSpeedEnvelope: Double
): EnvelopePart {
// Add maxBecDeltaSpeed to EBD curve overhead so it reaches a sufficiently high speed to
// guarantee that, after the speed translation, the corresponding EBI curve does intersect
// with envelope max speed.
val maxBecDeltaSpeed = maxBecDeltaSpeed()
val maxSpeedEbd = maxSpeedEnvelope + maxBecDeltaSpeed
val overhead =
Envelope.make(
EnvelopePart.generateTimes(
listOf(EnvelopeProfile.CONSTANT_SPEED),
doubleArrayOf(0.0, context.path.length),
doubleArrayOf(maxSpeedEbd, maxSpeedEbd)
)
)

val ebdCurve =
computeBrakingCurve(context, overhead, targetPosition, targetSpeed, BrakingType.ETCS_EBD)
assert(ebdCurve.beginPos >= 0.0 && ebdCurve.endPos >= targetPosition)
assert((ebdCurve.beginSpeed == maxSpeedEbd || ebdCurve.beginPos == 0.0))

val guiCurve =
computeBrakingCurve(context, overhead, targetPosition, targetSpeed, BrakingType.ETCS_GUI)
assert(guiCurve.beginPos >= 0.0 && guiCurve.endPos == targetPosition)
assert((guiCurve.beginSpeed == maxSpeedEbd || guiCurve.beginPos == 0.0))

val ebiCurve = computeEbiBrakingCurveFromEbd(context, ebdCurve, targetSpeed)
assert(ebiCurve.endSpeed == targetSpeed)

val fullIndicationCurve =
computeIndicationBrakingCurveFromRef(context, ebiCurve, BrakingCurveType.EBI, guiCurve)
assert(fullIndicationCurve.endSpeed == targetSpeed)
return fullIndicationCurve
}

/** Compute braking curve: used to compute EBD, SBD or GUI. */
private fun computeBrakingCurve(
context: EnvelopeSimContext,
Expand Down Expand Up @@ -282,15 +346,20 @@ private fun computeEbiBrakingCurveFromEbd(
targetSpeed: Double
): EnvelopePart {
val pointCount = ebdCurve.pointCount()
val newPositions = DoubleArray(pointCount)
val newSpeeds = DoubleArray(pointCount)
for (i in 0 until ebdCurve.pointCount()) {
var newPositions = DoubleArray(pointCount)
var newSpeeds = DoubleArray(pointCount)
for (i in 0 until pointCount) {
val speed = ebdCurve.getPointSpeed(i)
val becParams = computeBecParams(context, ebdCurve, speed, targetSpeed)
val newPos = ebdCurve.getPointPos(i) - becParams.dBec
val newSpeed = speed - becParams.deltaBecSpeed
newPositions[i] = newPos
newSpeeds[i] = newSpeed
newSpeeds[i] = max(newSpeed, 0.0)
if (newSpeed <= 0.0 && i < pointCount - 1) {
newPositions = newPositions.dropLast(pointCount - 1 - i).toDoubleArray()
newSpeeds = newSpeeds.dropLast(pointCount - 1 - i).toDoubleArray()
break
}
}

val fullBrakingCurve =
Expand Down Expand Up @@ -383,7 +452,7 @@ private fun keepBrakingCurveUnderOverlay(
val overlayBuilder =
ConstrainedEnvelopePartBuilder(
partBuilder,
PositionConstraint(beginPos, overlay.endPos),
PositionConstraint(max(beginPos, fullBrakingCurve.beginPos), overlay.endPos),
EnvelopeConstraint(overlay, EnvelopePartConstraintType.CEILING)
)
overlayBuilder.initEnvelopePart(positions[nbPoints - 1], speeds[nbPoints - 1], -1.0)
Expand Down

0 comments on commit 6212f99

Please sign in to comment.