Engineering/sysid_backseat.tl

#   Copyright (c) 2024 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.

mission sysid_backseat {
  """
  Universal system identification mission driven by a backseat application.

  The backseat publishes time-varying actuator commands and hold flags via
  customUri variables on the slate. This mission routes those commands to
  the appropriate Guidance behaviors and enforces a safety envelope.

  Supports all sysid test types: thrust model, elevator/rudder effectiveness,
  mass shifter/bladder trim, free response, and coupled MIMO excitation.
  """

  arguments {
    MissionTimeout = 2 hour
      """
      Maximum duration of mission.
      """

    TestName = "01_thrust"
      """
      Name of the test plan YAML to run (without .yaml extension).
      Written to the slate to trigger the backseat application.
      """

    Heading = Universal:platform_orientation
      """
      Vehicle heading to command. Always active during initial dive, only active
      during backseat test if heading_hold=true!
      """

    InitialDepth = 50 meter
      """
      Depth target for the initial dive before the backseat test starts.
      """

    TestDepth = InitialDepth
      """
      Depth to command during the backseat test. Only active if test has depth_hold=true!
      """

    InitialSpeed = 1.0 meter_per_second
      """
      Speed target for the initial dive.
      """

    DiveSettleTime = 30 second
      """
      Time to hold at InitialDepth before triggering the backseat test.
      Allows vehicle to stabilize at depth.
      """

    DepthDeadband = Control:VerticalControl.depthDeadband
      """
      Depth deadband for backseat test (doesn't apply in initial dive)
      """

    MaxDepth = 100 meter
      """
      Max depth for the entire mission.
      """

    MinAltitude = 15 meter
      """
      Min altitude for the entire mission.
      """

    MaxPitch = 60 degree
      """
      Hard maximum pitch limit (mission envelope). Vehicle will ignore backseat
      commands and enter safe mode if pitch exceeds this value.
      """

    ResumePitch = 30 degree
      """
      Pitch required to exit safe mode after hitting max pitch
      """

    MinOffshore = 2000 meter
      """
      Minimum offshore distance for the entire mission.
      """

    ElevatorDefault = 0 degree
      """
      Default elevator angle when not being commanded.
      """

    RudderDefault = 0 degree
      """
      Default rudder angle when not being commanded.
      """

    MassDefault = Control:VerticalControl.massDefault
      """
      Default mass position.
      """

    BuoyancyDefault = Control:VerticalControl.buoyancyNeutral
      """
      Default buoyancy position.
      """

    BackseatTimeout = 10 second
      """
      Max time allowed between backseat sysidActive updates.
      If exceeded, vehicle enters safe mode. This is independent of the normal
      backseat heartbeat timeout.
      """
  }

  output {
    Speed = InitialSpeed
      """
      Commanded vehicle speed (m/s).
      """

    ElevatorAngle = ElevatorDefault
      """
      Commanded elevator angle.
      """

    RudderAngle = RudderDefault
      """
      Commanded rudder angle.
      """

    MassPosition = MassDefault
      """
      Commanded mass position.
      """

    BuoyancyPosition = BuoyancyDefault
      """
      Commanded buoyancy position.
      """

    DepthTarget = InitialDepth
      """
      Commanded depth target.
      """

    HoldDepth = 1 count
      """
      Whether depth hold is active (1) or not (0).
      """

    HoldHeading = 1 count
      """
      Whether heading hold is active (1) or not (0).
      """

    HoldPitch = 1 count
      """
      Whether pitch hold is active (1) or not (0).
      """

    HoldSpeed = 1 count
      """
      Whether speed hold is active (1) or not (0).
      """

    SysidActive = 0 count
      """
      Whether backseat is actively commanding (1) or idle (0).
      """

    SafeMode = 0 count
      """
      1 while the mission is in safe mode (Speed=0, backseat commands ignored).
      Entered by pitch envelope trip or backseat timeout; exits once the
      relevant entry condition has cleared.
      """

    PitchSafeLatch = 0 count
      """
      1 while a pitch trip is latched. Set when |pitch| >= MaxPitch, cleared
      when |pitch| <= ResumePitch. While latched, SafeMode stays at 1 even if
      the backseat comes back online — prevents flapping near the limit.
      """
  }

  # Mission timeout.

  timeout duration=MissionTimeout

  insert id="NeedComms" Insert/NeedComms.tl

  # No NeedComms, only start/end of mission
  assign in sequence NeedComms:DiveInterval = MissionTimeout

  insert Insert/StandardEnvelopes.tl
  assign in sequence StandardEnvelopes:MinAltitude = MinAltitude
  assign in sequence StandardEnvelopes:MaxDepth = MaxDepth
  assign in sequence StandardEnvelopes:MinOffshore = MinOffshore

  # Pitch envelope is enforced at the mission level by the safe-mode aggregates
  # above (MaxPitch / ResumePitch). StandardEnvelopes:MaxPitch is intentionally
  # left at its default — the latching safe-mode behavior gives us the recovery
  # window we want, which StandardEnvelopes alone wouldn't provide.

  insert Insert/BackseatDriver.tl
  assign in sequence BackseatDriver:EnableBackseat = true

  insert Insert/PowerOnly.tl

  behavior Guidance:Execute {
    run in sequence

    set command = "set _.sysidTestCmd string \"init\""
  }

  # Pre-create the sysidComplete slot so WaitForComplete below has something
  # well-defined to compare against before the backseat ever publishes. Same
  # pattern/reasoning as the sysidTestCmd init above.
  # I think I don't actually need this, let's see
  # behavior Guidance:Execute {
  #   run in sequence

  #   set command = "set _.sysidComplete 0 count"
  # }

  # === SAFE MODE ===
  # One EnterSafeMode aggregate with OR'd entry conditions. While SafeMode==1
  # we force Speed=0 and ignore all command updates from the backseat
  #
  # PitchSafeLatch is a small separate state machine: it tracks the
  # MaxPitch / ResumePitch window so a pitch-induced safe mode doesn't clear
  # until pitch has actually come back inside ResumePitch to prevent flapping.

  aggregate PitchLatch_On {
    run when (
      PitchSafeLatch == 0 count
      and ( abs ( Universal:platform_pitch_angle ) >= MaxPitch )
    )

    assign in sequence PitchSafeLatch = 1 count
  }

  aggregate PitchLatch_Off {
    run when (
      PitchSafeLatch == 1 count
      and ( abs ( Universal:platform_pitch_angle ) <= ResumePitch )
    )

    assign in sequence PitchSafeLatch = 0 count
  }

  aggregate EnterSafeMode {
    run when (
      SafeMode == 0 count
      and (
        PitchSafeLatch == 1 count
        or (
          SysidActive == 1 count
          and ( elapsed ( customUri "_.sysidActive" ) >= BackseatTimeout )
        )
      )
    )

    assign in sequence SafeMode = 1 count
    assign in sequence SysidActive = 0 count
    assign in sequence Speed = 0 meter_per_second

    syslog fault "Safe mode ENTERED. pitch=" + Universal:platform_pitch_angle~degree
         + " deg, PitchSafeLatch=" + PitchSafeLatch~count
    #+ elapsed ( customUri "_.sysidActive" ) ~second + " s"
  }

  # Clear SafeMode when both entry conditions have cleared: pitch latch is off
  # AND the backseat is hearing fresh (re-armed). UpdateSysidActive below is
  # intentionally NOT gated on SafeMode so a returning backseat can drive this.
  aggregate ExitSafeMode {
    run when (
      SafeMode == 1 count
      and PitchSafeLatch == 0 count
      and SysidActive == 1 count
      and ( elapsed ( customUri "_.sysidActive" ) < BackseatTimeout )
    )

    assign in sequence SafeMode = 0 count

    syslog important "Safe mode EXITED. pitch=" + Universal:platform_pitch_angle~degree + " deg"
  }

  # === BACKSEAT UPDATE AGGREGATES ===
  # Poll each customUri; update output when backseat value is newer. All of the
  # command-channel updates are gated on SafeMode==0 so a still-publishing
  # backseat can't override our zero-speed safe state.

  aggregate UpdateSysidActive {
    run when (
      elapsed ( customUri "_.sysidActive" ) < ( elapsed ( SysidActive ) )
      and SysidActive != customUri "_.sysidActive"
    )

    assign in sequence SysidActive = customUri "_.sysidActive"
  }

  aggregate UpdateSpeed {
    run when (
      SafeMode == 0 count
      and ( elapsed ( customUri "_.speedCmd" ) < ( elapsed ( Speed ) ) )
      and ( Speed != customUri "_.speedCmd" )
    )

    assign in sequence Speed = customUri "_.speedCmd"
  }

  aggregate UpdateElevator {
    run when (
      SafeMode == 0 count
      and ( elapsed ( customUri "_.elevatorAngleCmd" ) < ( elapsed ( ElevatorAngle ) ) )
      and ( ElevatorAngle != customUri "_.elevatorAngleCmd" )
    )

    assign in sequence ElevatorAngle = customUri "_.elevatorAngleCmd"
  }

  aggregate UpdateRudder {
    run when (
      SafeMode == 0 count
      and ( elapsed ( customUri "_.rudderAngleCmd" ) < ( elapsed ( RudderAngle ) ) )
      and ( RudderAngle != customUri "_.rudderAngleCmd" )
    )

    assign in sequence RudderAngle = customUri "_.rudderAngleCmd"
  }

  aggregate UpdateMassPosition {
    run when (
      SafeMode == 0 count
      and ( elapsed ( customUri "_.massPositionCmd" ) < ( elapsed ( MassPosition ) ) )
      and ( MassPosition != customUri "_.massPositionCmd" )
    )

    assign in sequence MassPosition = customUri "_.massPositionCmd"
  }

  aggregate UpdateBuoyancyPosition {
    run when (
      SafeMode == 0 count
      and ( elapsed ( customUri "_.buoyancyPositionCmd" ) < ( elapsed ( BuoyancyPosition ) ) )
      and ( BuoyancyPosition != customUri "_.buoyancyPositionCmd" )
    )

    assign in sequence BuoyancyPosition = customUri "_.buoyancyPositionCmd"
  }

  # I'm going to leave this here for now, but actually I think we want the mission to command depth
  # so it's easier to change.
  # aggregate UpdateDepthTarget {
  #   run when (
  #     elapsed ( customUri "_.depthCmd" ) < ( elapsed ( DepthTarget ) )
  #     and ( DepthTarget != customUri "_.depthCmd" )
  #   )

  #   assign in sequence DepthTarget = customUri "_.depthCmd"
  # }

  aggregate UpdateHoldDepth {
    run when (
      SafeMode == 0 count
      and ( elapsed ( customUri "_.holdDepth" ) < ( elapsed ( HoldDepth ) ) )
      and ( HoldDepth != customUri "_.holdDepth" )
    )

    assign in sequence HoldDepth = customUri "_.holdDepth"
  }

  aggregate UpdateHoldHeading {
    run when (
      SafeMode == 0 count
      and ( elapsed ( customUri "_.holdHeading" ) < ( elapsed ( HoldHeading ) ) )
      and ( HoldHeading != customUri "_.holdHeading" )
    )

    assign in sequence HoldHeading = customUri "_.holdHeading"
  }

  aggregate UpdateHoldPitch {
    run when (
      SafeMode == 0 count
      and ( elapsed ( customUri "_.holdPitch" ) < ( elapsed ( HoldPitch ) ) )
      and ( HoldPitch != customUri "_.holdPitch" )
    )

    assign in sequence HoldPitch = customUri "_.holdPitch"
  }

  aggregate UpdateHoldSpeed {
    run when (
      SafeMode == 0 count
      and ( elapsed ( customUri "_.holdSpeed" ) < ( elapsed ( HoldSpeed ) ) )
      and ( HoldSpeed != customUri "_.holdSpeed" )
    )

    assign in sequence HoldSpeed = customUri "_.holdSpeed"
  }

  call id="StartingMission" priorityHere=false refId="NeedComms"

  # === INITIAL DIVE ===
  # Dive to InitialDepth under mission control before handing off to backseat.

  aggregate InitialDive {
    run in sequence

    syslog important "Beginning initial dive to " + InitialDepth~meter + " at "
         + InitialSpeed~meter_per_second

    behavior Guidance:SetSpeed {
      run in parallel

      set speed = InitialSpeed
    }

    behavior Guidance:Point {
      run in parallel

      set heading = Heading
    }

    behavior Guidance:Pitch {
      run in sequence

      set depth = InitialDepth
    }

    # Settle at depth
    behavior Guidance:Wait {
      run in sequence

      set duration = DiveSettleTime
    }

    syslog important "Initial dive complete. Triggering backseat test: " + TestName~none_str
  }

  # Trigger the backseat test by writing the test name to the slate.
  # Use Guidance:Execute with `set ... string ...` rather than
  # `assign in sequence customUri ... = TestName` because the assign form
  # pre-types the customUri slot as numeric at mission load (TethysL can't
  # infer string type from an arg without a unit declaration), which then
  # blocks any later string write. The Execute path defers slot creation
  # to runtime with an explicit `string` type. Pattern from
  # lrauv-backseat-marl/Missions/marl4.tl:285-288.
  behavior Guidance:Execute {
    run in sequence

    #set command = "set _.sysidTestCmd string \"sysid_01_thrust_model\""
    set command = "set _.sysidTestCmd string \"" + TestName + "\""
  }

  # === GUIDANCE BEHAVIORS ===
  # Each behavior is conditionally active based on the hold flags.
  # When a hold is ON, the closed-loop controller manages that axis.
  # When a hold is OFF, direct actuator commands from the backseat are used.

  aggregate SysidControl {
    run in sequence

    break if ( customUri "_.sysidComplete" == 1 count )

    assign in parallel Control:VerticalControl.depthDeadband = DepthDeadband
    assign in sequence DepthTarget = TestDepth

    # --- Speed: always active (no direct RPM control) ---
    behavior Guidance:SetSpeed {
      run in parallel

      set speed = Speed
    }

    # --- Buoyancy and mass: always commanded.
    # MassPosition and BuoyancyPosition default to MassDefault/BuoyancyDefault
    # (see the output declarations) and only move when a phase publishes a new
    # value on the slate — so "holding" is implicit for tests that don't step
    # the shifter or bladder.
    behavior Guidance:Buoyancy {
      run in parallel

      set position = BuoyancyPosition
    }

    behavior Guidance:Mass {
      run in parallel

      set position = MassPosition
    }

    # --- Depth hold: closed-loop depth control ---
    aggregate DepthHoldActive {
      run while ( HoldDepth == 1 count )

      behavior Guidance:Pitch {
        run in parallel

        set depth = DepthTarget
      }
    }

    # --- Pitch hold: closed-loop pitch control ---
    aggregate PitchHoldActive {
      run while (
        HoldPitch == 1 count
        and HoldDepth == 0 count
      )

      behavior Guidance:Pitch {
        run in parallel

        set pitch = 0 degree
      }
    }

    # --- Direct elevator: when depth and pitch holds are OFF ---
    aggregate DirectElevator {
      run while (
        HoldDepth == 0 count
        and HoldPitch == 0 count
      )

      behavior Guidance:Pitch {
        run in parallel

        set elevatorAngle = ElevatorAngle
      }
    }

    # --- Heading hold: closed-loop heading control ---
    aggregate HeadingHoldActive {
      run while ( HoldHeading == 1 count )

      behavior Guidance:Point {
        run in parallel

        set forceUpdate = true
        set heading = Heading
      }
    }

    # --- Direct rudder: when heading hold is OFF ---
    aggregate DirectRudder {
      run while ( HoldHeading == 0 count )

      behavior Guidance:Point {
        run in parallel

        set forceUpdate = true
        set rudderAngle = RudderAngle
      }
    }

    behavior Guidance:Wait {
      run in sequence

      set duration = MissionTimeout
    }
  }

  call id="PhoneHome" priorityHere=false refId="NeedComms"
}