Insert/NeedCommsTransit.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.

aggregate NeedCommsTransit {
  arguments {
    DiveIntervalMax = 1 hour
      """
      Elapsed time after most recent surfacing when vehicle will begin to
      ascend to the surface again. The timing is actually based on the
      variable Universal:time_fix instead of the variable
      Universal:platform_communications because the latter is not toggled
      until the message queue is clear. As a result, there are situations
      where the vehicle might dive for the first half of a yo, then return to
      the surface to continue communications, rendering the communications
      timeout useless. When adjusting this parameter, do not use "set", use
      Assign in a mission, or set NeedCommsTransit:DiveInterval from the command
      line.
      """

    DiveIntervalTransit = 1 hour

    WaitForPitchUp = 10 minute
      """
      Extra time to wait for the vehicle to pitch up (avoid truncating a yo).
      """

    SurfacePitch = 20 degree
      """
      Pitch to maintain while ascending
      """

    SurfaceDepthRate = NaN meter_per_second
      """
      Depth rate to maintain while ascending. Set to NaN if using pitch
      """

    SurfaceSpeed = 1 meter_per_second
      """
      Standard speed during surfacing. Don't reduce this too much below 1 m/s
      -- the elevators can stall. (At 0.5 m/s, we have observed evidence of
      stall in the past.)
      """

    SurfacingTimeout = 1000 second
      """
      The maximum time allowed for the ascent to surface.
      """

    GPSTimeout = 7 minute
      """
      Maximum amount of time to spend trying to get each GPS fix.
      """

    CommsTimeout = 30 minute
      """
      Maximum amount of time to spend on the surface trying to communicate
      before giving up, getting another GPS fix, and diving again.
      """

    Transit = false

    TransitSpeed = NaN meter_per_second

    TransitDepth = NaN meter

    TransitLat = NaN degree

    TransitLon = NaN degree
  }

  output {
    StartedSurfacing = false
  }

  run when (
    called
    or ( elapsed ( Universal:time_fix ) > ( DiveIntervalMax + WaitForPitchUp ) )
    or (
      elapsed ( Universal:time_fix ) > DiveIntervalMax
      and ( Universal:platform_pitch_angle > 0 degree )
    )
  )

  assign in sequence StartedSurfacing = true

  aggregate CommsTransit {
    run in sequence

    aggregate Comms {
      run in sequence

      syslog info "last time_fix was: " + Universal:time_fix~epoch_second

      behavior Guidance:GoToSurface {
        run in progression

        set pitch = SurfacePitch
        set depthRate = SurfaceDepthRate
        set speed = SurfaceSpeed
        set surfaceTimeout = SurfacingTimeout
      }

      readDatum {
        timeout duration=GPSTimeout {
          syslog important "first GPS update timeout"
        }

        Universal:time_fix
      }

      readDatum {
        timeout duration=CommsTimeout {
          syslog important "Comms timed out after " + CommsTimeout~minute + "minutes"
        }

        Universal:platform_communications
      }

      readDatum {
        timeout duration=GPSTimeout {
          syslog important "second GPS update timeout"
        }

        Universal:time_fix
      }

      aggregate ResetStartedSurfacing {
        run in sequence

        break if ( Transit )

        assign in sequence StartedSurfacing = false
      }
    }

    aggregate Transit {
      run in sequence

      break if (
        not ( Transit )
        or (
          not (
            ( TransitLat >= 0 degree
              or ( TransitLat < 0 degree )
            )
            and (
              TransitLon >= 0 degree
              or ( TransitLon < 0 degree )
            )
          )
        )
        or ( not ( TransitDepth > 0 meter ) )
        or not ( TransitSpeed > 0 meter_per_second )
      )

      aggregate CommsInTransit {
        run when (
          ( elapsed ( Universal:time_fix ) > ( DiveIntervalTransit + WaitForPitchUp ) )
          or (
            elapsed ( Universal:time_fix ) > DiveIntervalTransit
            and ( Universal:platform_pitch_angle > 0 degree )
          )
        )

        behavior Guidance:GoToSurface {
          run in progression

          set pitch = SurfacePitch
          set depthRate = SurfaceDepthRate
          set speed = SurfaceSpeed
          set surfaceTimeout = SurfacingTimeout
        }

        readDatum {
          timeout duration=GPSTimeout {
            syslog important "first GPS update timeout"
          }

          Universal:time_fix
        }

        readDatum {
          timeout duration=CommsTimeout {
            syslog important "Comms timed out after " + CommsTimeout~minute + "minutes"
          }

          Universal:platform_communications
        }

        readDatum {
          timeout duration=GPSTimeout {
            syslog important "second GPS update timeout"
          }

          Universal:time_fix
        }
      }

      aggregate TransitWaypoint {
        run in sequence

        behavior Guidance:SetSpeed {
          run in parallel

          set speed = TransitSpeed
        }

        behavior Guidance:Mass {
          run in parallel

          set position = Control:VerticalControl.massDefault
        }

        behavior Guidance:Buoyancy {
          run in parallel

          set position = Control:VerticalControl.buoyancyNeutral
        }

        behavior Guidance:Pitch {
          run in parallel

          set depth = TransitDepth
        }

        behavior Guidance:Waypoint {
          run in sequence

          set latitude = TransitLat
          set longitude = TransitLon
        }

        assign in sequence StartedSurfacing = false
      }
    }
  }
}