Skip to content

Commit

Permalink
Merge pull request #2415 from bjonkman/f/Envision_updates
Browse files Browse the repository at this point in the history 
Lidar bug fix + other minor changes
  • Loading branch information
@andrew-platt
andrew-platt authored Sep 12, 2024
2 parents 2246bef + 7825609 commit 32ad558
Show file tree
Hide file tree
Showing 24 changed files with 249 additions and 334 deletions.
3 changes: 0 additions & 3 deletions glue-codes/simulink/src/FAST_SFunc.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -189,9 +189,6 @@ static void mdlInitializeSizes(SimStruct *S)
InitInputAry[i] = AdditionalInitInputs[i + 1];
}
}
else{
InitInputAry[0] = SensorType_None; // tell it not to use lidar (shouldn't be necessary, but we'll cover our bases)
}

// set this before possibility of error in Fortran library:

Expand Down
4 changes: 2 additions & 2 deletions modules/hydrodyn/src/HydroDyn_Input.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -385,11 +385,11 @@ SUBROUTINE HydroDyn_ParseInput( InputFileName, OutRootName, FileInfo_In, InputFi
! read the table entries AxCoefID, AxCd, AxCa, AxCp, AxFdMod, AxVnCOff, AxFDLoFSc in the HydroDyn input file
! Try reading in 7 entries first
call ParseAry( FileInfo_In, CurLine, ' axial coefficients line '//trim( Int2LStr(I)), tmpReArray, size(tmpReArray), ErrStat2, ErrMsg2, UnEc )
if ( ErrStat2 /= 0 ) then ! Try reading in 5 entries
if ( ErrStat2 /= ErrID_None ) then ! Try reading in 5 entries
tmpReArray(6) = -1.0 ! AxVnCoff
tmpReArray(7) = 1.0 ! AxFDLoFSc
call ParseAry( FileInfo_In, CurLine, ' axial coefficients line '//trim( Int2LStr(I)), tmpReArray(1:5), 5, ErrStat2, ErrMsg2, UnEc )
if ( ErrStat2 /= 0 ) then ! Try reading in 4 entries
if ( ErrStat2 /= ErrID_None ) then ! Try reading in 4 entries
tmpReArray(5) = 0.0 ! AxFdMod
call ParseAry( FileInfo_In, CurLine, ' axial coefficients line '//trim( Int2LStr(I)), tmpReArray(1:4), 4, ErrStat2, ErrMsg2, UnEc )
if (Failed()) return;
Expand Down
10 changes: 5 additions & 5 deletions modules/hydrodyn/src/YawOffset.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ MODULE YawOffset
MODULE PROCEDURE GetRotAngsD
END INTERFACE GetRotAngs

INTERFACE WrapToPi
INTERFACE WrapToPi ! See NWTC_Num.f90:: mpi2pi()
MODULE PROCEDURE WrapToPiR
MODULE PROCEDURE WrapToPiD
END INTERFACE WrapToPi
Expand Down Expand Up @@ -238,31 +238,31 @@ FUNCTION WrapTo180R(angle)

REAL(SiKi), INTENT(IN) :: angle
REAL(SiKi) :: WrapTo180R
WrapTo180R = modulo(angle + 180.0, 360.0) - 180.0
WrapTo180R = modulo(angle + 180.0_SiKi, 360.0_SiKi) - 180.0_SiKi

END FUNCTION WrapTo180R

FUNCTION WrapTo180D(angle)

REAL(R8Ki), INTENT(IN) :: angle
REAL(R8Ki) :: WrapTo180D
WrapTo180D = modulo(angle + 180.0, 360.0) - 180.0
WrapTo180D = modulo(angle + 180.0_R8Ki, 360.0_R8Ki) - 180.0_R8Ki

END FUNCTION WrapTo180D

FUNCTION WrapToPiR(angle)

REAL(SiKi), INTENT(IN) :: angle
REAL(SiKi) :: WrapToPiR
WrapToPiR = modulo(angle + PI, TwoPi) - PI
WrapToPiR = modulo(angle + Pi_R4, TwoPi_R4) - Pi_R4

END FUNCTION WrapToPiR

FUNCTION WrapToPiD(angle)

REAL(R8Ki), INTENT(IN) :: angle
REAL(R8Ki) :: WrapToPiD
WrapToPiD = modulo(angle + PI, TwoPi) - PI
WrapToPiD = modulo(angle + Pi_R8, TwoPi_R8) - Pi_R8

END FUNCTION WrapToPiD

Expand Down
25 changes: 5 additions & 20 deletions modules/inflowwind/src/InflowWind_Subs.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -490,22 +490,18 @@ SUBROUTINE InflowWind_ParseInputFileInfo( InputFileData, InFileInfo, PriPath, In

! LIDAR Sensor Type
CALL ParseVar( InFileInfo, CurLine, "SensorType", InputFileData%SensorType, TmpErrStat, TmpErrMsg, UnEc )
CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
IF (Failed()) RETURN

! Number of Range Gates
CALL ParseVar( InFileInfo, CurLine, "NumPulseGate", InputFileData%NumPulseGate, TmpErrStat, TmpErrMsg, UnEc )
CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
IF (Failed()) RETURN

! Pulse Gate Spacing
CALL ParseVar( InFileInfo, CurLine, "PulseSpacing", InputFileData%PulseSpacing, TmpErrStat, TmpErrMsg, UnEc )
CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
IF (Failed()) RETURN

! NumBeam: Number of points to output the lidar measured wind velocity (1 to 5)
CALL ParseVar( InFileInfo, CurLine, "NumBeam", InputFileData%NumBeam, TmpErrStat, TmpErrMsg, UnEc )
CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
if (Failed()) return

! Before proceeding, make sure that NumBeam makes sense
Expand All @@ -515,53 +511,42 @@ SUBROUTINE InflowWind_ParseInputFileInfo( InputFileData, InFileInfo, PriPath, In
RETURN
ELSE
! Allocate space for the output location arrays:
CALL AllocAry( InputFileData%FocalDistanceX, InputFileData%NumBeam, 'NumBeam', TmpErrStat, TmpErrMsg )
CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
CALL AllocAry( InputFileData%FocalDistanceY, InputFileData%NumBeam, 'NumBeam', TmpErrStat, TmpErrMsg )
CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
CALL AllocAry( InputFileData%FocalDistanceZ, InputFileData%NumBeam, 'NumBeam', TmpErrStat, TmpErrMsg )
CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
CALL AllocAry( InputFileData%FocalDistanceX, InputFileData%NumBeam, 'FocalDistanceX', TmpErrStat, TmpErrMsg ); CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
CALL AllocAry( InputFileData%FocalDistanceY, InputFileData%NumBeam, 'FocalDistanceY', TmpErrStat, TmpErrMsg ); CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
CALL AllocAry( InputFileData%FocalDistanceZ, InputFileData%NumBeam, 'FocalDistanceZ', TmpErrStat, TmpErrMsg ); CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
if (Failed()) return
ENDIF

! Focal Distance X
CALL ParseAry( InFileInfo, CurLine, 'FocalDistanceX', InputFileData%FocalDistanceX, InputFileData%NumBeam, TmpErrStat, TmpErrMsg, UnEc )
CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
if (Failed()) return

! Focal Distance Y
CALL ParseAry( InFileInfo, CurLine, 'FocalDistanceY', InputFileData%FocalDistanceY, InputFileData%NumBeam, TmpErrStat, TmpErrMsg, UnEc )
CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
if (Failed()) return

! Focal Distance Z
CALL ParseAry( InFileInfo, CurLine, 'FocalDistanceZ', InputFileData%FocalDistanceZ, InputFileData%NumBeam, TmpErrStat, TmpErrMsg, UnEc )
CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
if (Failed()) return

! Rotor Apex Offset Position
CALL ParseAry( InFileInfo, CurLine, "RotorApexOffsetPos", InputFileData%RotorApexOffsetPos, 1, TmpErrStat, TmpErrMsg, UnEc )
CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
CALL ParseAry( InFileInfo, CurLine, "RotorApexOffsetPos", InputFileData%RotorApexOffsetPos, size(InputFileData%RotorApexOffsetPos), TmpErrStat, TmpErrMsg, UnEc )
IF (Failed()) RETURN

! URefIni
CALL ParseVar( InFileInfo, CurLine, "URefLid", InputFileData%URefLid, TmpErrStat, TmpErrMsg, UnEc )
CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
IF (Failed()) RETURN

! Measurement Interval
CALL ParseVar( InFileInfo, CurLine, "MeasurementInterval", InputFileData%MeasurementInterval, TmpErrStat, TmpErrMsg, UnEc )
CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
IF (Failed()) RETURN

! Lidar Radial Vel
CALL ParseLoVar( InFileInfo, CurLine, "LidRadialVel", InputFileData%LidRadialVel, TmpErrStat, TmpErrMsg, UnEc )
CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
CALL ParseVar( InFileInfo, CurLine, "LidRadialVel", InputFileData%LidRadialVel, TmpErrStat, TmpErrMsg, UnEc )
IF (Failed()) RETURN

! Consider Hub Motion
CALL ParseVar( InFileInfo, CurLine, "ConsiderHubMotion", InputFileData%ConsiderHubMotion, TmpErrStat, TmpErrMsg, UnEc )
CALL SetErrStat( TmpErrStat, TmpErrMsg, ErrStat, ErrMsg, RoutineName )
IF (Failed()) RETURN


Expand Down
15 changes: 5 additions & 10 deletions modules/inflowwind/src/Lidar.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -336,10 +336,9 @@ SUBROUTINE Lidar_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMs

REAL(ReKi) :: Distance(3) ! distance vector between input measurement and lidar positions

REAL(ReKi) :: LidPosition(3) ! Lidar Position
REAL(ReKi) :: LidPosition_N(3) !Transformed Lidar Position
REAL(ReKi) :: LidarMsrPosition(3) !Transformed Lidar Position
REAL(ReKi) :: MeasurementCurrentStep
REAL(ReKi) :: LidPosition(3) ! Lidar Position
REAL(ReKi) :: LidarMsrPosition(3) !Transformed Lidar Position
REAL(ReKi) :: MeasurementCurrentStep


REAL(ReKi) :: PositionXYZ(3,2)
Expand All @@ -366,13 +365,9 @@ SUBROUTINE Lidar_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMs
RETURN
ENDIF

LidPosition = p%lidar%LidPosition + p%lidar%RotorApexOffsetPos ! lidar offset-from-rotor-apex position
IF (p%lidar%ConsiderHubMotion == 1) THEN
LidPosition_N = (/ u%lidar%HubDisplacementX, u%lidar%HubDisplacementY, u%lidar%HubDisplacementZ /) & ! rotor apex position (absolute)
+ p%lidar%RotorApexOffsetPos ! lidar offset-from-rotor-apex position
LidPosition = p%lidar%LidPosition + LidPosition_N
ELSE
LidPosition_N = p%lidar%RotorApexOffsetPos
LidPosition = p%lidar%LidPosition + LidPosition_N
LidPosition = LidPosition + (/ u%lidar%HubDisplacementX, u%lidar%HubDisplacementY, u%lidar%HubDisplacementZ /) ! rotor apex position (absolute)
END IF

IF (p%lidar%SensorType == SensorType_None) RETURN
Expand Down
2 changes: 1 addition & 1 deletion modules/map/src/bstring/bstraux.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@
#include "config.h"
#endif

#ifdef _MSC_VER
#if defined _MSC_VER && !defined _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif

Expand Down
2 changes: 1 addition & 1 deletion modules/map/src/bstring/bstrlib.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@
#include "config.h"
#endif

#if defined (_MSC_VER)
#if defined _MSC_VER && !defined _CRT_SECURE_NO_WARNINGS
/* These warnings from MSVC++ are totally pointless. */
# define _CRT_SECURE_NO_WARNINGS
#endif
Expand Down
4 changes: 4 additions & 0 deletions modules/map/src/mapapi.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,10 @@ extern "C"
{
#endif

#if defined _MSC_VER && !defined _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif

// Some redefinitions from MAP_Types.h, so the API does not need to exposes the
// internal data structures.
typedef struct MAP_InputType* MAP_Input_t;
Expand Down
2 changes: 1 addition & 1 deletion modules/nwtc-library/src/ModMesh.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3396,7 +3396,7 @@ SUBROUTINE CreateInputPointMesh(mesh, posInit, orientInit, errStat, errMsg, hasM
endif
if (hasMotion) then
mesh%Orientation = mesh%RefOrientation
mesh%TranslationDisp = 0.0_ReKi
mesh%TranslationDisp = 0.0_R8Ki
mesh%TranslationVel = 0.0_ReKi
mesh%RotationVel = 0.0_ReKi
endif
Expand Down
36 changes: 17 additions & 19 deletions modules/nwtc-library/src/NWTC_Num.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6482,22 +6482,21 @@ FUNCTION TaitBryanYXZExtractR8(M) result(theta)


END FUNCTION TaitBryanYXZExtractR8


FUNCTION TaitBryanYXZConstructR4(theta) result(M)
! this function creates a rotation matrix, M, from a 1-2-3 rotation
! sequence of the 3 TaitBryan angles, theta_x, theta_y, and theta_z, in radians.
! M represents a change of basis (from global to local coordinates;
! not a physical rotation of the body). it is the inverse of TaitBryanYXZExtract().
!
! M = R(theta_z) * R(theta_x) * R(theta_y)
! = [ cz sz 0 | [ 1 0 0 | [ cy 0 -sy |
! |-sz cz 0 |* | 0 cx sx | * | 0 1 0 |
! | 0 0 1 ] | 0 -sx cx ] | sy 0 cy ]
! = [ cy*cz+sy*sx*sz cx*sz cy*sx*sz-cz*sy |
! |cz*sy*sx-cy*sz cx*cz cy*cz*sx+sy*sz |
! |cx*sy -sx cx*cy ]
! where cz = cos(theta_z), sz = sin(theta_z), cy = cos(theta_y), etc.
!=======================================================================
!> this function creates a rotation matrix, M, from a 1-2-3 rotation
!! sequence of the 3 TaitBryan angles, theta_x, theta_y, and theta_z, in radians.
!! M represents a change of basis (from global to local coordinates;
!! not a physical rotation of the body). it is the inverse of TaitBryanYXZExtract().
!!
!! M = R(theta_z) * R(theta_x) * R(theta_y)
!! = [ cz sz 0 | [ 1 0 0 | [ cy 0 -sy |
!! |-sz cz 0 |* | 0 cx sx | * | 0 1 0 |
!! | 0 0 1 ] | 0 -sx cx ] | sy 0 cy ]
!! = [ cy*cz+sy*sx*sz cx*sz cy*sx*sz-cz*sy |
!! |cz*sy*sx-cy*sz cx*cz cy*cz*sx+sy*sz |
!! |cx*sy -sx cx*cy ]
!! where cz = cos(theta_z), sz = sin(theta_z), cy = cos(theta_y), etc.
PURE FUNCTION TaitBryanYXZConstructR4(theta) result(M)

REAL(SiKi) :: M(3,3) !< rotation matrix, M
REAL(SiKi), INTENT(IN) :: theta(3) !< the 3 rotation angles: \f$\theta_x, \theta_y, \theta_z\f$
Expand Down Expand Up @@ -6531,8 +6530,8 @@ FUNCTION TaitBryanYXZConstructR4(theta) result(M)
M(3,3) = cy*cx

END FUNCTION TaitBryanYXZConstructR4
FUNCTION TaitBryanYXZConstructR8(theta) result(M)
!=======================================================================
PURE FUNCTION TaitBryanYXZConstructR8(theta) result(M)

! this function creates a rotation matrix, M, from a 1-2-3 rotation
! sequence of the 3 TaitBryan angles, theta_x, theta_y, and theta_z, in radians.
Expand Down Expand Up @@ -6580,7 +6579,6 @@ FUNCTION TaitBryanYXZConstructR8(theta) result(M)
M(3,3) = cy*cx

END FUNCTION TaitBryanYXZConstructR8


!=======================================================================
!> This routine takes an array of time values such as that returned from
Expand Down
Loading

0 comments on commit 32ad558

Please sign in to comment.