Engineering/control_test_straight.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 control_test_straight {
  """
  Vehicle drives towards the specified waypoint or at the specified heading
  spending a fixed duration at each of up to five depths, using some
  combination of actuators for control.
  """

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

    Repeat = 1 count
      """
      Number of times the vehicle should try to cycle through depths.
      """

    Lat = NaN degree
      """
      Latitude of waypoint. Uses heading if NaN.
      """

    Lon = NaN degree
      """
      Longitude of waypoint. Uses heading if NaN.
      """

    Heading[1..5] = NaN degree
      """
      If waypoint is NaN, absolute heading at which to drive for leg {$}.
      """

    LegDuration = 10 minute
      """
      Duration of each leg.
      """

    Depth[1..5] = NaN meter
      """
      Depth of leg {$}. NaN to skip leg.
      """

    Speed[1..5] = 1 meter_per_second
      """
      Speed of leg {$}.
      """

    UseElevators = true
      """
      Whether or not to use elevators for control
      """

    ElevatorDefault = 0 degree
      """
      Elevator setpoint if UseElevators is false
      """

    UseMass = false
      """
      Whether or not to use mass for control
      """

    MassDefault = Control:VerticalControl.massDefault
      """
      Mass setpoint if UseMass is false
      """

    UseBuoyancy = false
      """
      Whether or not to use buoyancy for control
      """

    BuoyancyDefault = Control:VerticalControl.buoyancyNeutral
      """
      Buoyancy setpoint if UseBuoyancy is false
      """

    SinkOffSurface = false
      """
      If true, use buoyancy only to decend to SinkDepth before driving to first target depth
      """

    SinkDepth = 3 meter
      """
      Initial depth to decend using only buoyancy before driving
      """

    SinkDepthTimeout = 10 minute
      """
      Max wait time for the vehicle to reach the target depth
      """

    MinAltitude = 7 meter
      """
      Minimum height above the sea floor for the entire mission.
      """

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

    NeedCommsTime = 1 hour
      """
      How often to surface for comms. Will interrupt legs, should probably equal MissionTimeout.
      """
  }

  output {
    ReachedWaypoint = false

    DoingComms = false

    NeedsSink = SinkOffSurface
  }

  timeout duration=MissionTimeout

  # Run science, because why not.
  insert Insert/Science.tl

  insert id="NeedComms" Insert/NeedComms.tl

  insert Insert/StandardEnvelopes.tl

  assign in sequence StandardEnvelopes:MinAltitude = MinAltitude

  assign in sequence StandardEnvelopes:MaxDepth = MaxDepth

  insert Insert/BackseatDriver.tl

  insert Insert/PowerOnly.tl

  call id="StartingMission" refId="NeedComms"

  aggregate SurfaceComms {
    run when (
      called
      or ( elapsed ( Universal:time_fix ) > NeedCommsTime )
    )

    assign in sequence DoingComms = true

    call refId="NeedComms"

    assign in sequence DoingComms = false
    assign in sequence NeedsSink = SinkOffSurface
  }

  aggregate DoSink {
    run when ( called )

    break if ( not SinkOffSurface )

    # Fix fixed vertical control actuators
    behavior Guidance:Buoyancy id="BuoyancyHold" {
      run while ( NeedsSink )

      set position = NaN cc
    }

    behavior Guidance:SetSpeed {
      run while ( NeedsSink )

      set speed = 0 m/s
    }

    syslog info "Sinking to " + SinkDepth~meter

    behavior Guidance:Pitch {
      run in sequence

      timeout duration=SinkDepthTimeout {
        syslog important "Timed out while trying to reach target sink depth at current depth of "
             + Universal:depth~meter
      }

      set depth = SinkDepth
    }

    assign in sequence NeedsSink = false
  }

  # Set up fixed waypoint control, heading can be varied by leg
  aggregate UseWaypoint {
    run while (
      not isNaN ( Lat )
      and not isNaN ( Lon )
      and not ReachedWaypoint
    )

    behavior Guidance:Waypoint {
      run in sequence

      set latitude = Lat
      set longitude = Lon
    }

    assign in sequence ReachedWaypoint = true
  }

  # Fix fixed vertical control actuators
  behavior Guidance:Buoyancy id="BuoyancyHold" {
    run while (
      not UseBuoyancy
      and not NeedsSink
    )

    set position = BuoyancyDefault
  }

  behavior Guidance:Pitch id="MassHold" {
    run while ( not UseMass )

    set massPosition = MassDefault
  }

  behavior Guidance:Pitch id="ElevatorHold" {
    run while ( not UseElevators )

    set elevatorAngle = ElevatorDefault
  }

  # Vary leg depths and speeds
  aggregate Lap {
    run in sequence repeat=Repeat

    break if ( ReachedWaypoint )

    aggregate InitialSink {
      run while (
        NeedsSink
        and not DoingComms
      )

      call refId="DoSink"
    }

    macro $i = 1..5 {
      aggregate Leg$i {
        run in sequence

        break if (
          isNaN ( Depth[$i] )
          or ReachedWaypoint
        )

        syslog info "Running leg at depth" + Depth[$i]~meter

        behavior Guidance:Point {
          run while (
            isNaN ( Lat )
            and isNaN ( Lon )
            and not isNaN ( Heading[$i] )
          )

          set heading = Heading[$i]
        }

        behavior Guidance:SetSpeed {
          run in parallel

          set speed = Speed[$i]
        }

        behavior Guidance:Pitch {
          run in parallel

          set depth = Depth[$i]
        }

        behavior Guidance:Wait {
          run in sequence

          timeout duration=LegDuration
        }
      }
    }
  }
}