Science/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 = 4 hour
      """
      Maximum duration of mission.
      """

    BackseatTimeout = 10 second
      """
      Max time allowed between backseat sysidActive updates.
      If exceeded, vehicle enters safe mode.
      """

    MaxDepth = 200 meter
      """
      Hard maximum depth limit (mission envelope).
      """

    MinAltitude = 7 meter
      """
      Hard minimum altitude limit (mission envelope).
      """

    MaxPitch = 30 degree
      """
      Hard maximum pitch limit (mission envelope).
      """

    MaxRoll = 30 degree
      """
      Hard maximum roll limit (mission envelope).
      """

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

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

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

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

    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.
      """
  }

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

    Heading = 0 degree
      """
      Commanded vehicle heading.
      """

    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).
      """

    ElapsedSinceMissionStarted = 0 minute
  }

  # Mission timeout.

  timeout duration=MissionTimeout

  # Init Heading at current vehicle heading.

  assign in sequence Heading = Universal:platform_orientation

  behavior Guidance:Execute {
    run in sequence

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

  # === BACKSEAT UPDATE AGGREGATES ===
  # Poll each customUri; update output when backseat value is newer.

  aggregate BackseatTimeout {
    run when (
      SysidActive == 1 count
      and ( elapsed ( customUri "_.sysidActive" ) >= BackseatTimeout )
    )

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

    syslog fault "Backseat timeout! Setting safe mode."
  }

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

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

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

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

  aggregate UpdateHeading {
    run when (
      elapsed ( customUri "_.headingCmd" ) < ( elapsed ( Heading )
        and (
          customUri "_.headingCmd" == Heading
          or (
            customUri "_.headingCmd" == 0 degree
            and Heading == 0 degree
          )
        )
      )
    )

    assign in sequence Heading = customUri "_.headingCmd"
  }

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

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

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

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

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

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

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

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

  aggregate UpdateDepthTarget {
    run when (
      elapsed ( customUri "_.depthCmd" ) < ( elapsed ( DepthTarget )
        and ( DepthTarget != customUri "_.depthCmd" )
      )
    )

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

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

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

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

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

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

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

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

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

  # === BACKSEAT POWER ===

  insert Insert/BackseatDriver.tl

  assign in sequence BackseatDriver:EnableBackseat = true

  insert Insert/PowerOnly.tl

  # === SAFETY ENVELOPES ===

  insert Insert/StandardEnvelopes.tl

  assign in sequence StandardEnvelopes:MinAltitude = MinAltitude
  assign in sequence StandardEnvelopes:MaxDepth = MaxDepth

  # TODO limit pitch and roll
  # assign in sequence StandardEnvelopes:MaxPitch = MaxPitch
  # assign in sequence StandardEnvelopes:MaxRoll = MaxRoll

  # === 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

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

      set speed = Speed
    }

    # --- Buoyancy: always active ---
    behavior Guidance:Buoyancy {
      run in parallel

      set position = BuoyancyPosition
    }

    # --- Mass: always active ---
    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
      }
    }

    # --- Periodic check-in for comms ---
    aggregate CheckIn {
      run in sequence repeat=576

      behavior Guidance:Wait {
        run in sequence

        set duration = MissionTimeout
      }
    }
  }
}