Skip to content

RegressionTests/testAltDpthEnvPtchBehavior.tl

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
mission TestMission {
  test_code {{{
    r2d = 180 / 3.14159
    upAngle = 25
    downAngle = -25
    delta = 4.5
    initDelay = 150 * 2.5
    upDownDelayUntil= 150
    upDownStopAt= 225
    upDelayCount = 0
    downDelayCount = 0
    upAngleTotal = 0
    upAngleCount = 0
    downAngleTotal = 0
    downAngleCount = 0
    data = slate.read("platform_pitch_angle")[0]
    for i in range(len(data)):
      angle = float(data[i].data)
      if( i < initDelay ):
        continue
      if angle >= 0:
        if downDelayCount > 0:
          downDelayCount = 0
          upDelayCount = 1
        else:
          upDelayCount = upDelayCount + 1
        if( upDelayCount >= upDownDelayUntil and upDelayCount < upDownStopAt ):
          upAngleTotal = upAngleTotal + angle
          upAngleCount = upAngleCount + 1
      else:
        if upDelayCount > 0:
          upDelayCount = 0
          downDelayCount = 1
        else:
          downDelayCount = downDelayCount + 1
        if( downDelayCount >= upDownDelayUntil and downDelayCount < upDownStopAt ):
          downAngleTotal = downAngleTotal + angle
          downAngleCount = downAngleCount + 1

    if upAngleCount > 0 and downAngleCount > 0:
      upAngleAvg = upAngleTotal / upAngleCount * r2d
      downAngleAvg = downAngleTotal / downAngleCount * r2d
      if utils.withinDelta(upAngleAvg, upAngle, delta) and \
         utils.withinDelta(downAngleAvg, downAngle, delta):
        passed = True
      else:
        if not utils.withinDelta(upAngleAvg, upAngle, delta):
          print("|upAngleAvg(%f) - upAngle(%f)| > delta(%f)" % (upAngleAvg, upAngle, delta) )
        if not utils.withinDelta(downAngleAvg, downAngle, delta):
          print("|downAngleAvg(%f) - downAngle(%f)| > delta(%f)" % (downAngleAvg, downAngle, delta) )
    else:
      print("upAngleCount = %d, downAngleCount = %d" % (upAngleCount, downAngleCount) )
  }}}

  arguments {
    UpAngle = 25 degree

    DownAngle = -25 degree
  }

  timeout duration=P2H

  insert RegressionTests/InsertHighPriority.xml

  insert RegressionTests/InsertSurfaceOps.xml

  aggregate TestAggregate {
    run in sequence

    behavior Guidance:AltitudeEnvelope {
      run in parallel

      set minAltitude = 6 meter
    }

    behavior Guidance:DepthEnvelope {
      run in parallel

      set minDepth = 5 meter
      set maxDepth = 270 meter
      set upPitch = UpAngle
      set downPitch = DownAngle
    }

    behavior Guidance:SetSpeed {
      run in parallel

      set speed = 1 meter_per_second
    }

    behavior Guidance:YoYo {
      run in parallel

      set upPitch = UpAngle
      set downPitch = DownAngle
    }

    behavior Guidance:Waypoint {
      run in sequence

      set latitudeDelta = -0.015 degree
      set longitudeDelta = -0.015 degree
    }
  }
}