Engineering/joystick_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 {
  """
  This mission drives the vehicle on the surface at a speed
  and heading as commanded by a beackseat computer.

  The mission monitors the time between speed updates and
  stops the vehicle if SpeedUpdateTimeout expires.
  """

  arguments {
    # Mission-wide settings

    MissionTimeout = 2 hour
      """
      Maximum duration of mission
      """

    SpeedUpdateTimeout = 15 second
      """
      Max time allowed between speed command updates.
      """

    HorizontalCommandMode = 0 count
      """
      Mission horizontal command mode. 0 = Heading command (default). 1 = Rudder angle command.
      """

    ElevatorAngle = -7 degree
      """
      Holds elevator angle to keep vehicle pitched up on the surface.
      """
  }

  output {
    # Mission args do not change these.

    Speed = 0 meter_per_second
      """
      Commanded vehicle speed.
      """

    Heading = 0 degree
      """
      Commanded vehicle heading. Mission will init Heading to current vehicle heading.
      """

    RudderAngle = 0 degree
      """
      Commanded vehicle RudderAngle. Only acitve when mission is in rudder commands mode.
      """

    MassPosition = Control:VerticalControl.massDefault
      """
      Commanded vehicle mass position. Mission will init MassPosition to current vehicle default.
      """

    HeadingMode = 0 count
      """
      Heading angle command mode enumaration.
      """

    RudderMode = 1 count
      """
      Rudder angle command mode enumaration.
      """

    ElapsedSinceMissionStarted = 0 minute
  }

  # Missions should always start with a timeout.

  timeout duration=MissionTimeout

  # Init Heading at current vehicle heading.

  assign in sequence Heading = Universal:platform_orientation

  # Add guidance update directives high in the stack.

  aggregate SpeedTimeout {
    run when ( elapsed ( Speed ) >= SpeedUpdateTimeout )

    assign in sequence Speed = 0 meter_per_second

    syslog fault "Speed timeout reached. Stopping vehicle (speed " + Speed~meter_per_second + ")."
  }

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

    assign in sequence Speed = customUri "_.speedCmd"

    syslog info "Updating vehicle speed " + Speed~meter_per_second
  }

  aggregate UpdateCommandMode {
    run when (
      elapsed ( customUri "_.horizontalCmdMode" ) < ( elapsed ( HorizontalCommandMode ) )
      and (
        customUri "_.horizontalCmdMode" == HeadingMode
        or ( customUri "_.horizontalCmdMode" == RudderMode )
      )
    )

    assign in sequence HorizontalCommandMode = customUri "_.horizontalCmdMode"

    syslog info "Updating horizontal command mode " + HorizontalCommandMode~count
  }

  aggregate UpdateHeading {
    run when (
      elapsed ( customUri "_.headingCmd" ) < ( elapsed ( Heading ) )
    )

    assign in sequence Heading = customUri "_.headingCmd"

    syslog info "Updating heading " + Heading~degree
  }

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

    assign in sequence RudderAngle = customUri "_.rudderAngleCmd"

    syslog info "Updating rudder angle " + RudderAngle~degree
  }

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

    assign in sequence MassPosition = customUri "_.massPositionCmd"

    syslog info "Updating mass position " + MassPosition~millimeter
  }

  # Power the Backseat payload.

  insert Insert/BackseatDriver.tl

  assign in sequence BackseatDriver:EnableBackseat = true

  insert Insert/PowerOnly.tl

  # Keep the vehilce on the surface.

  aggregate Surface {
    run while (
      Universal:depth >= Control:VerticalControl.surfaceThreshold
    )

    behavior Guidance:GoToSurface {
      run in progression
    }
  }

  # Start mission sequence.

  aggregate JoystickControl {
    run in sequence

    behavior Guidance:Buoyancy {
      run in parallel

      set position = Control:VerticalControl.buoyancyDefault
    }

    behavior Guidance:SetSpeed {
      run in parallel

      set speed = Speed
    }

    behavior Guidance:Mass {
      run in parallel

      set position = MassPosition
    }

    behavior Guidance:Pitch {
      run in parallel

      set elevatorAngle = ElevatorAngle
    }

    aggregate CmdHeading {
      run while ( HorizontalCommandMode == HeadingMode )

      behavior Guidance:Point {
        run in parallel

        set forceUpdate = true
        set heading = Heading
      }
    }

    aggregate CmdRudder {
      run while ( HorizontalCommandMode == RudderMode )

      behavior Guidance:Point {
        run in parallel

        set forceUpdate = true
        set rudderAngle = RudderAngle
      }
    }

    aggregate CheckIn {
      run in sequence repeat=288

      readDatum id="Read_GPS" {
        timeout duration=P5M

        Universal:time_fix
      }

      readDatum id="Read_Iridium" {
        Universal:platform_communications
      }

      behavior Guidance:Wait {
        run in sequence

        set duration = 5 minute
      }

      assign in sequence ElapsedSinceMissionStarted = elapsed ( Universal:mission_started )

      syslog important "Joystick mission has been running for " + ElapsedSinceMissionStarted~minute
    }
  }
}