Insert/TerrainAvoidance.tl

#   Copyright (c) 2026 MBARI
#   MBARI Proprietary Information. Confidential. All Rights Reserved
#   Unauthorized copying or distribution of this file via any medium is strictly
#   prohibited.
#
#   WARNING - This file contains information whose export is restricted by the
#   Export Administration Act of 1979 (Title 50, U.S.C., App. 2401 et seq.), as
#   amended. Violations of these export laws are subject to severe civil and/or
#   criminal penalties.

aggregate TerrainAvoidance {
  """
  Terrain avoidance: runs the vertical overrides that the backseat TA module commands.
  Use with Insert/StandardEnvelopes.tl. Insert it below StandardEnvelopes and BackseatDriver.
  TA commands only with Shadow = false here and shadow: false in the backseat config.
  Backseat sends _.verticalCmdMode (-1 none, 1 stop, 2 depth, 5 pitch, 100 back off),
  _.depthCmd (meter) and _.pitchCmd (radian, positive = nose up).
  """

  arguments {
    VerticalCmdTimeout = 15 second
      """Max time without a backseat command before the override is cancelled."""

    ReverseSpeed = -1 meter_per_second
      """Astern speed during a back-off."""

    PublishPeriod = 2 second
      """How often the TA params below are re-published to the backseat."""

    # Backseat module params, published as _.ta*. The backseat rejects all four
    # altitudes unless abort <= min <= caution and min <= target.
    TargetAltitude = 35 meter
      """Fly height above the seafloor (module h_target). Keep above MinAltitude."""

    CautionAltitude = 25 meter
      """Arms a blind climb below this when both sensors drop out (module h_caution). Keep above MinAltitude."""

    AvoidMinAltitude = 15 meter
      """Module escalates below this (module h_min). Keep above MinAltitude."""

    AbortAltitude = 12 meter
      """Below this, the backseat backs off, then the insert stops the mission (only with AllowAbort; module h_abort). Keep above MinAltitude."""

    MaxClimbAngle = 20 degree
      """Max climb angle (module gamma_max_deg)."""

    LookAheadRange = 100 meter
      """Terrain look-ahead range (module profile_range_m)."""

    NominalSpeed = 1 meter_per_second
      """Cruise speed the module assumes (module nominal_speed_mps)."""

    MinActiveDepth = 5 meter
      """Shallower than this, the backseat starts no override and releases one in progress (a started back-off completes). A pitch override also stops here (module min_active_depth_m)."""

    Shadow = true
      """true = backseat only decides and logs. false = allow TA commands (backseat config must allow them too)."""

    AllowAbort = false
      """false = TA starts no back-off or stop; it climbs instead. A back-off already started still ends in a stop."""
  }

  output {
    VerticalOverrideMode = -1 count
    DepthSetpoint = NaN meter
    PitchAngle = NaN radian
    ParamsLogged = false
    Armed = false

    # Last backseat mode logged as ignored, so that each one is logged once.
    IgnoredMode = -1 count

    # Same codes as _.verticalCmdMode.
    NormalMode = -1 count
    StopMode = 1 count
    DepthMode = 2 count
    PitchMode = 5 count
    BackoffMode = 100 count
  }

  run in parallel

  # The slate var age is reset by every message, also one with an unchanged mode.
  aggregate VerticalCmdTimeout {
    run when (
      elapsed ( customUri "_.verticalCmdMode" ) >= VerticalCmdTimeout
      and VerticalOverrideMode != NormalMode
    )

    assign in sequence VerticalOverrideMode = NormalMode

    syslog fault "TerrainAvoidance: backseat went silent during an override, returning control to the mission."
  }

  # Only commands received after arming count, so a stale command from an
  # earlier mission or from shadow is never used.
  aggregate Arm {
    run when ( not Shadow and not Armed )

    assign in sequence DepthSetpoint = NaN meter
    assign in sequence PitchAngle = NaN radian
    assign in sequence IgnoredMode = NormalMode
    assign in sequence Armed = true
  }

  aggregate Disarm {
    run when ( Shadow and Armed )

    assign in sequence Armed = false
    assign in sequence IgnoredMode = NormalMode
  }

  aggregate ModeCmd {
    run while (
      Armed
      and not Shadow
      and elapsed ( customUri "_.verticalCmdMode" ) < elapsed ( Armed )
      and elapsed ( customUri "_.verticalCmdMode" ) < VerticalCmdTimeout
    )

    aggregate UpdateVerticalMode {
      run when (
        customUri "_.verticalCmdMode" != VerticalOverrideMode
        and ( customUri "_.verticalCmdMode" == NormalMode
              or ( ( AllowAbort or VerticalOverrideMode == BackoffMode )
                   and customUri "_.verticalCmdMode" == StopMode )
              or customUri "_.verticalCmdMode" == DepthMode
              or customUri "_.verticalCmdMode" == PitchMode
              or ( AllowAbort and customUri "_.verticalCmdMode" == BackoffMode ) )
      )

      assign in sequence VerticalOverrideMode = customUri "_.verticalCmdMode"
      assign in sequence IgnoredMode = NormalMode

      syslog important "TerrainAvoidance: mode updated to " + VerticalOverrideMode~count
    }

    # The latched mode sent again ends a run of ignored modes, so the next one is logged.
    aggregate ClearIgnored {
      run when (
        customUri "_.verticalCmdMode" == VerticalOverrideMode
        and IgnoredMode != NormalMode
      )

      assign in sequence IgnoredMode = NormalMode
    }

    # For example from an older backseat.
    aggregate UnknownCmd {
      run when (
        customUri "_.verticalCmdMode" != IgnoredMode
        and customUri "_.verticalCmdMode" != NormalMode
        and customUri "_.verticalCmdMode" != StopMode
        and customUri "_.verticalCmdMode" != DepthMode
        and customUri "_.verticalCmdMode" != PitchMode
        and customUri "_.verticalCmdMode" != BackoffMode
      )

      assign in sequence VerticalOverrideMode = NormalMode
      assign in sequence IgnoredMode = customUri "_.verticalCmdMode"

      syslog fault "TerrainAvoidance: unknown backseat mode " + IgnoredMode~count + ", override released."
    }

    # A back-off already latched still ends in a stop.
    aggregate AbortDisabledCmd {
      run when (
        not AllowAbort
        and VerticalOverrideMode != BackoffMode
        and VerticalOverrideMode != StopMode
        and customUri "_.verticalCmdMode" != IgnoredMode
        and ( customUri "_.verticalCmdMode" == StopMode
              or customUri "_.verticalCmdMode" == BackoffMode )
      )

      assign in sequence IgnoredMode = customUri "_.verticalCmdMode"

      syslog important "TerrainAvoidance: AllowAbort = false, ignoring backseat mode " + IgnoredMode~count
    }
  }

  aggregate UpdateDepth {
    run when (
      Armed
      and elapsed ( customUri "_.depthCmd" ) < elapsed ( Armed )
      and elapsed ( customUri "_.depthCmd" ) < elapsed ( DepthSetpoint )
    )

    assign in sequence DepthSetpoint = customUri "_.depthCmd"
  }

  aggregate UpdatePitch {
    run when (
      Armed
      and elapsed ( customUri "_.pitchCmd" ) < elapsed ( Armed )
      and elapsed ( customUri "_.pitchCmd" ) < elapsed ( PitchAngle )
    )

    assign in sequence PitchAngle = customUri "_.pitchCmd"
  }

  aggregate ShadowIgnoreCmd {
    run when (
      Shadow
      and elapsed ( customUri "_.verticalCmdMode" ) < VerticalCmdTimeout
      and customUri "_.verticalCmdMode" != IgnoredMode
    )

    assign in sequence IgnoredMode = customUri "_.verticalCmdMode"

    syslog important "TerrainAvoidance: Shadow = true, ignoring backseat mode " + IgnoredMode~count
  }

  aggregate ShadowRelease {
    run when (
      Shadow
      and VerticalOverrideMode != NormalMode
    )

    assign in sequence VerticalOverrideMode = NormalMode

    syslog important "TerrainAvoidance: Shadow = true, override released."
  }

  # Re-publish so a backseat that starts late still gets the params.
  aggregate PublishTAParams {
    run when (
      elapsed ( customUri "_.taTargetAltitude" ) > PublishPeriod )

    assign in sequence customUri "_.taTargetAltitude" = TargetAltitude
    assign in sequence customUri "_.taCautionAltitude" = CautionAltitude
    assign in sequence customUri "_.taMinAltitude" = AvoidMinAltitude
    assign in sequence customUri "_.taAbortAltitude" = AbortAltitude
    assign in sequence customUri "_.taMaxClimbAngle" = MaxClimbAngle
    assign in sequence customUri "_.taLookAheadRange" = LookAheadRange
    assign in sequence customUri "_.taNominalSpeed" = NominalSpeed
    assign in sequence customUri "_.taMinActiveDepth" = MinActiveDepth
    assign in sequence customUri "_.taShadow" = Shadow
    assign in sequence customUri "_.taAllowAbort" = AllowAbort
  }

  aggregate LogTAParams {
    run when ( not ParamsLogged )

    syslog info "TerrainAvoidance params: target " + TargetAltitude~meter
      + ", caution " + CautionAltitude~meter
      + ", min " + AvoidMinAltitude~meter
      + ", abort " + AbortAltitude~meter
      + ", climb " + MaxClimbAngle~degree
      + ", range " + LookAheadRange~meter
      + ", speed " + NominalSpeed~meter_per_second
      + ", min active depth " + MinActiveDepth~meter
      + ", shadow " + Shadow~bool
      + ", allow abort " + AllowAbort~bool

    assign in sequence ParamsLogged = true
  }

  aggregate StopMission {
    run when ( VerticalOverrideMode == StopMode )

    syslog critical "TerrainAvoidance: backseat abort, stopping mission."
  }

  # Hockey stop. No pitch angle: astern with the nose up moves the vehicle down.
  aggregate BackoffOverride {
    run while ( VerticalOverrideMode == BackoffMode and not isNaN ( DepthSetpoint ) )

    behavior Guidance:SetSpeed {
      run in parallel

      set speed = ReverseSpeed
    }

    behavior Guidance:Pitch {
      run in parallel

      set depth = DepthSetpoint
    }
  }

  aggregate DepthOverride {
    run while ( VerticalOverrideMode == DepthMode and not isNaN ( DepthSetpoint ) )

    behavior Guidance:Pitch {
      run in parallel

      set depth = DepthSetpoint
    }
  }

  # Backstop: the backseat itself releases shallower than MinActiveDepth.
  aggregate PitchOverride {
    run while (
      VerticalOverrideMode == PitchMode
      and not isNaN ( PitchAngle )
      and Universal:depth > MinActiveDepth
    )

    behavior Guidance:Pitch {
      run in parallel

      set pitch = PitchAngle
    }
  }
}