Add WSE inputs to debug, force IMU filter to initialize at 0 and add HPF

Ben-Mertz · Jul 16, 2020 · a0ed80b · a0ed80b
1 parent d214f5a
commit a0ed80b
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 9 deletions.
diff --git a/src/ControllerBlocks.f90 b/src/ControllerBlocks.f90
@@ -144,6 +144,11 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar)
         REAL(4), DIMENSION(3,1), SAVE   :: K        ! Kalman gain matrix
         REAL(4)                         :: R_m      ! Measurement noise covariance [(rad/s)^2]
 
+        ! ---- Debug Inputs ------
+        DebugVar%WE_b   = LocalVar%PC_PitComTF*R2D
+        DebugVar%WE_w   = LocalVar%RotSpeedF
+        DebugVar%WE_t   = LocalVar%VS_LastGenTrqF
+
         ! ---- Define wind speed estimate ---- 
 
         ! Inversion and Invariance Filter implementation

diff --git a/src/Filters.f90 b/src/Filters.f90
@@ -274,8 +274,13 @@ SUBROUTINE PreFilterMeasuredSignals(CntrPar, LocalVar, objInst)
 
         ! Filtering the tower fore-aft acceleration signal 
         IF (CntrPar%Fl_Mode == 1) THEN
-            LocalVar%NacIMU_FA_AccF = SecLPFilter(LocalVar%NacIMU_FA_Acc, LocalVar%DT, CntrPar%F_FlCornerFreq, CntrPar%F_FlDamping, LocalVar%iStatus, .FALSE., objInst%instSecLPF) ! Fixed Damping
-            ! LocalVar%NacIMU_FA_AccF = HPFilter(LocalVar%NacIMU_FA_AccF, LocalVar%DT, CntrPar%F_FlCornerFreq/3, LocalVar%iStatus, .FALSE., objInst%instHPF) 
+            ! Force to start at 0
+            IF (LocalVar%iStatus == 0) THEN
+                LocalVar%NacIMU_FA_AccF = SecLPFilter(0., LocalVar%DT, CntrPar%F_FlCornerFreq, CntrPar%F_FlDamping, LocalVar%iStatus, .FALSE., objInst%instSecLPF) ! Fixed Damping
+            ELSE
+                LocalVar%NacIMU_FA_AccF = SecLPFilter(LocalVar%NacIMU_FA_Acc, LocalVar%DT, CntrPar%F_FlCornerFreq, CntrPar%F_FlDamping, LocalVar%iStatus, .FALSE., objInst%instSecLPF) ! Fixed Damping
+            ENDIF
+                LocalVar%NacIMU_FA_AccF = HPFilter(LocalVar%NacIMU_FA_AccF, LocalVar%DT, 0.0167, LocalVar%iStatus, .FALSE., objInst%instHPF) 
             ! LocalVar%NacIMU_FA_AccF = NotchFilterSlopes(LocalVar%NacIMU_FA_Acc, LocalVar%DT, CntrPar%F_FlCornerFreq, CntrPar%F_FlDamping, LocalVar%iStatus, .FALSE., objInst%instNotchSlopes) ! Fixed Damping
             IF (CntrPar%F_NotchType >= 2) THEN
                 LocalVar%NACIMU_FA_AccF = NotchFilter(LocalVar%NacIMU_FA_AccF, LocalVar%DT, CntrPar%F_NotchCornerFreq, CntrPar%F_NotchBetaNumDen(1), CntrPar%F_NotchBetaNumDen(2), LocalVar%iStatus, .FALSE., objInst%instNotch) ! Fixed Damping

diff --git a/src/Functions.f90 b/src/Functions.f90
@@ -471,27 +471,29 @@ SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, size_avcOUTNAME
 
         ! Set up Debug Strings and Data
         ! Note that Debug strings have 10 character limit
-        nDebugOuts = 9
+        nDebugOuts = 11
         ALLOCATE(DebugOutData(nDebugOuts))
         !                 Header                            Unit                                Variable
-        DebugOutStr1  = 'IMU_FA_AccF';     DebugOutUni1  = '(m/s)';      DebugOutData(1)  = LocalVar%NacIMU_FA_AccF
+        DebugOutStr1  = 'FA_AccF';         DebugOutUni1  = '(m/s)';      DebugOutData(1)  = LocalVar%NacIMU_FA_AccF
         DebugOutStr2  = 'WE_Vw';           DebugOutUni2  = '(rad)';      DebugOutData(2)  = LocalVar%WE_Vw
-        DebugOutStr3  = 'IMU_FA_Acc';      DebugOutUni3  = '(rad/s^2)';  DebugOutData(3)  = LocalVar%NacIMU_FA_Acc
+        DebugOutStr3  = 'FA_AccR';          DebugOutUni3  = '(rad/s^2)';  DebugOutData(3)  = LocalVar%NacIMU_FA_Acc
         DebugOutStr4  = 'FA_Acc';          DebugOutUni4  = '(m/s^2)';    DebugOutData(4)  = LocalVar%FA_Acc
         DebugOutStr5  = 'Fl_Pitcom';       DebugOutUni5  = '(rad)';      DebugOutData(5)  = LocalVar%Fl_Pitcom
         DebugOutStr6  = 'WE_Cp';           DebugOutUni6  = '(-)';        DebugOutData(6)  = DebugVar%WE_Cp
         DebugOutStr7  = 'PC_MinPit';       DebugOutUni7  = '(rad)';      DebugOutData(7)  = LocalVar%PC_MinPit
         DebugOutStr8  = 'SS_dOmF';         DebugOutUni8  = '(rad/s)';    DebugOutData(8)  = LocalVar%SS_DelOmegaF
-        DebugOutStr9  = 'WE_Pitch';        DebugOutUni8  = '(rad)';      DebugOutData(9)  = DebugVar%WE_Pitch
+        DebugOutStr9  = 'WE_b';            DebugOutUni9  = '(deg)';      DebugOutData(9)  = DebugVar%WE_b
+        DebugOutStr10  = 'WE_t';            DebugOutUni10  = '(Nm)';      DebugOutData(10)  = DebugVar%WE_t
+        DebugOutStr11  = 'WE_w';            DebugOutUni11  = '(rad/s)';      DebugOutData(11)  = DebugVar%WE_w
 
         Allocate(DebugOutStrings(nDebugOuts))
         Allocate(DebugOutUnits(nDebugOuts))
         DebugOutStrings =   [CHARACTER(10)  :: DebugOutStr1, DebugOutStr2, DebugOutStr3, DebugOutStr4, &
                                                 DebugOutStr5, DebugOutStr6, DebugOutStr7, DebugOutStr8, &
-                                                DebugOutStr9]
+                                                DebugOutStr9, DebugOutStr10, DebugOutStr11]
         DebugOutUnits =     [CHARACTER(10)  :: DebugOutUni1, DebugOutUni2, DebugOutUni3, DebugOutUni4, &
                                                 DebugOutUni5, DebugOutUni6, DebugOutUni7, DebugOutUni8, &
-                                                DebugOutUni9]
+                                                DebugOutUni9, DebugOutUni10, DebugOutUni11]
 
         ! Initialize debug file
         IF (LocalVar%iStatus == 0)  THEN  ! .TRUE. if we're on the first call to the DLL

diff --git a/src/ROSCO_Types.f90 b/src/ROSCO_Types.f90
@@ -219,7 +219,9 @@ MODULE ROSCO_Types
 
 TYPE, PUBLIC :: DebugVariables
     REAL(4)                             :: WE_Cp                        ! Cp that WSE uses to determine aerodynamic torque, for debug purposes [-]
-    REAL(4)                             :: WE_Pitch                       ! Cp that WSE uses to determine aerodynamic torque, for debug purposes [-]
+    REAL(4)                             :: WE_b                       ! Pitch that WSE uses to determine aerodynamic torque, for debug purposes [-]
+    REAL(4)                             :: WE_w                       ! Rotor Speed that WSE uses to determine aerodynamic torque, for debug purposes [-]
+    REAL(4)                             :: WE_t                      ! Torque that WSE uses, for debug purposes [-]
 END TYPE DebugVariables
 
 END MODULE ROSCO_Types