trap nans in low/stopped rotor speeds

nikhar-abbas · Jun 22, 2021 · 1129adc · 1129adc
1 parent 72e7076
commit 1129adc
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 6 deletions.
diff --git a/src/ControllerBlocks.f90 b/src/ControllerBlocks.f90
@@ -131,6 +131,8 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar)
         REAL(8)                 :: Tau_r            ! Estimated rotor torque [Nm]
         REAL(8)                 :: a                ! wind variance
         REAL(8)                 :: lambda           ! tip-speed-ratio [rad]
+        REAL(8)                 :: RotSpeed         ! Rotor Speed [rad], locally
+
         !           - Covariance matrices
         REAL(8), DIMENSION(3,3)         :: F        ! First order system jacobian 
         REAL(8), DIMENSION(3,3), SAVE   :: P        ! Covariance estiamte 
@@ -168,11 +170,11 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar)
             Q = RESHAPE((/0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0/),(/3,3/))
             IF (LocalVar%iStatus == 0) THEN
                 ! Initialize recurring values
-                om_r = LocalVar%RotSpeedF
+                om_r = max(LocalVar%RotSpeedF, CntrPar%VS_MinOMSpd)
                 v_t = 0.0
                 v_m = LocalVar%HorWindV
                 v_h = LocalVar%HorWindV
-                lambda = LocalVar%RotSpeed * CntrPar%WE_BladeRadius/v_h
+                lambda = max(LocalVar%RotSpeed, CntrPar%VS_MinOMSpd) * CntrPar%WE_BladeRadius/v_h
                 xh = RESHAPE((/om_r, v_t, v_m/),(/3,1/))
                 P = RESHAPE((/0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 1.0/),(/3,3/))
                 K = RESHAPE((/0.0,0.0,0.0/),(/3,1/))
@@ -183,7 +185,7 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar)
                 A_op = interp1d(CntrPar%WE_FOPoles_v,CntrPar%WE_FOPoles,v_h)
 
                 ! TEST INTERP2D
-                lambda = LocalVar%RotSpeed * CntrPar%WE_BladeRadius/v_h
+                lambda = max(LocalVar%RotSpeed, CntrPar%VS_MinOMSpd) * CntrPar%WE_BladeRadius/v_h
                 Cp_op = interp2d(PerfData%Beta_vec,PerfData%TSR_vec,PerfData%Cp_mat, LocalVar%BlPitch(1)*R2D, lambda )
                 Cp_op = max(0.0,Cp_op)
 

diff --git a/src/Functions.f90 b/src/Functions.f90
@@ -195,7 +195,7 @@ REAL FUNCTION interp2d(xData, yData, zData, xq, yq)
     !       4| a    b   c
     !       5| d    e   f
     !       6| g    H   i
-
+        USE ieee_arithmetic
         IMPLICIT NONE
         ! Inputs
         REAL(8), DIMENSION(:),   INTENT(IN)     :: xData        ! Provided x data (vector), to find query point (should be monotonically increasing)
@@ -215,7 +215,7 @@ REAL FUNCTION interp2d(xData, yData, zData, xq, yq)
 
         ! ---- Find corner indices surrounding desired interpolation point -----
             ! x-direction
-        IF (xq <= MINVAL(xData)) THEN       ! On lower x-bound, just need to find zData(yq)
+        IF (xq <= MINVAL(xData) .OR. (ieee_is_nan(xq))) THEN       ! On lower x-bound, just need to find zData(yq)
             j = 1
             jj = 1
             interp2d = interp1d(yData,zData(:,j),yq)
@@ -241,7 +241,7 @@ REAL FUNCTION interp2d(xData, yData, zData, xq, yq)
         ENDIF
         j = j-1 ! Move j back one
             ! y-direction
-        IF (yq <= MINVAL(yData)) THEN       ! On lower y-bound, just need to find zData(xq)
+        IF (yq <= MINVAL(yData) .OR. (ieee_is_nan(yq))) THEN       ! On lower y-bound, just need to find zData(xq)
             i = 1
             ii = 1
             interp2d = interp1d(xData,zData(i,:),xq)