Commit 6c5280f8 authored by davidovitch's avatar davidovitch
Browse files

track user defined signal as input for IPC

parent b0234aa5
Pipeline #13969 passed with stage
in 1 minute and 8 seconds
module cyclic_pitch_controller_mod
use BuildInfo
use cyclic_pitch_controller_fcns_mod
use iso_c_binding
implicit none
contains
!**************************************************************************************************
......@@ -174,4 +175,253 @@ array2(10) =switchfactor
return
end subroutine update_cyclic_pitch_controller
!**************************************************************************************************
!**************************************************************************************************
subroutine init_cpc_track(array1,array2) bind(c,name='init_cpc_track')
! Applies perturbation to control signals
implicit none
!DEC$ IF .NOT. DEFINED(__LINUX__)
!DEC$ ATTRIBUTES DLLEXPORT :: init_cpc_track
!GCC$ ATTRIBUTES DLLEXPORT :: init_cpc_track
!DEC$ END IF
real*8 array1(30),array2(1)
! Input parameters
! 1: constant 1 ; Lead angle [deg]
! 2: constant 2 ; Proportional gain at zero pitch [deg/Nm]
! 3: constant 3 ; Integral gain at zero pitch [deg/(Nm*s)]
! 4: constant 4 ; Differential gain at zero pitch [deg*s/Nm]
! 5: constant 5 ; Coefficient of linear term in aerodynamic gain scheduling, KK1 [deg]
! 6: constant 6 ; Coefficient of quadratic term in aerodynamic gain scheduling, KK2 [deg^2]
! ; (if zero, KK1 = pitch angle at double gain)
! 7: constant 7 ; Low-pass filter frequency [Hz]
! 8: constant 8 ; Low-pass filter damping ratio [-]
! 9: constant 9 ; Low-pass filter time constant for gain scheduling [s]
! 10: constant 10 ; Maximum amplitude on cyclic pitch [deg]
! 11: constant 11 ; Thresshold for full power switch [-]
! 12: constant 12 ; Start time for activting the ipc [s]
! 13: constant 13 ; is the input in rotating [1] or non-rotating coordinates [0]?
! 14: constant 14 ; element (1,1) from the coupling matrix
! 15: constant 15 ; element (1,2) from the coupling matrix
! 16: constant 16 ; element (1,3) from the coupling matrix
! 17: constant 17 ; element (2,1) from the coupling matrix
! 18: constant 18 ; element (2,2) from the coupling matrix
! 19: constant 19 ; element (2,3) from the coupling matrix
! 20: constant 20 ; element (3,1) from the coupling matrix
! 21: constant 21 ; element (3,2) from the coupling matrix
! 22: constant 22 ; element (3,3) from the coupling matrix
! write the dll version info to the command windows
call echo_version()
! get values from input array1
LeadAngle = array1(1)*degrad
PID_cos_var%Kpro = array1(2) !*degrad
PID_cos_var%Kint = array1(3) !*degrad
PID_cos_var%Kdif = array1(4) !*degrad
PID_cos_var%velmax = 0.d0 ! not used
! PID Var:
!real(mk) Kpro,Kdif,Kint,outmin,outmax,velmax,error1,outset1,outres1
!integer :: stepno1 = 0
!real(mk) outset,outpro,outdif,error1_old,outset1_old,outres1_old,outres
if (array1(5).eq.0.d0) then
invkk1=0.d0
else
invkk1=1.d0/(array1(5)*degrad)
endif
if (array1(6).eq.0.d0) then
invkk2=0.d0
else
invkk2=1.d0/(array1(6)*degrad*degrad)
endif
LP2_cos_var%f0 = array1(7)
LP2_cos_var%zeta = array1(8)
LP1_pit_var%tau = array1(9)
PID_cos_var%outmin =-array1(10)*degrad
PID_cos_var%outmax = array1(10)*degrad
thr_ratcyclic = array1(11)
time_on = array1(12)
input_coord = array1(13)
! The two PID controllers are the same
LP2_sin_var = LP2_cos_var
PID_sin_var = PID_cos_var
LP1_rpm_var = LP1_pit_var
! Initialize the transformation matrices
call setup_mbc_matrices
! populate the Jacobian coupling matrix
Jinv(1,1)=array1(14)
Jinv(1,2)=array1(15)
Jinv(1,3)=array1(16)
Jinv(2,1)=array1(17)
Jinv(2,2)=array1(18)
Jinv(2,3)=array1(19)
Jinv(3,1)=array1(20)
Jinv(3,2)=array1(21)
Jinv(3,3)=array1(22)
! Switch filter
LP1_status_var%tau=10.d0
! dummy array
array2 = 0.d0
return
end subroutine init_cpc_track
!**************************************************************************************************
subroutine update_cpc_track(array1,array2) bind(c,name='update_cpc_track')
implicit none
!DEC$ IF .NOT. DEFINED(__LINUX__)
!DEC$ ATTRIBUTES DLLEXPORT :: update_cpc_track
!GCC$ ATTRIBUTES DLLEXPORT :: update_cpc_track
!DEC$ END IF
real*8 array1(50),array2(20)
! Input array1 must contains
!
! 1: general time [s]
! 2: Azimuth angle of blade 1 (zero = blade up) in [rad] [-pi,+pi]
! 3: Rotor speed [rad/s]
! 4: user defined input signal 1st component
! 5: user defined input signal 2nd component
! 6: user defined input signal 3rd component
! 7: Pitch angle reference of blade 1 from collective pitch controller [rad]
! 8: Pitch angle reference of blade 2 from collective pitch controller [rad]
! 9: Pitch angle reference of blade 3 from collective pitch controller [rad]
! 10: Status flag from collective pitch controller [0=normal operation] (dll type2_dll dtu_we_controller inpvec 22)
! 11: Pullpower switch from main ctrl (dll type2_dll dtu_we_controller inpvec 14)
!
! Output array2 contains
!
! 1 : Pitch angle reference of blade 1 [rad] without mean cpc anlge;
! 2 : Pitch angle reference of blade 2 [rad] without mean cpc anlge;
! 3 : Pitch angle reference of blade 3 [rad] without mean cpc anlge;
! 4 : user defined input signal 1st component inpvec_mbc(1);
! 5 : user defined input signal 2nd component inpvec_mbc(2);
! 6 : user defined input signal 3rd component inpvec_mbc(3);
! 7 : LP+Notch cos filtered inpvec(2);
! 8 : LP+Notch sin filtered inpvec(3);
! 9 : pitchref_mbc mean;
! 10 : pitchref_mbc cos;
! 11 : pitchref_mbc sin;
! 12 : switch factor;
! 13 : matmul(Jinv pitref_mbc) mean;
! 14 : matmul(Jinv pitref_mbc) Cosine;
! 15 : matmul(Jinv pitref_mbc) Sine;
! 16 : gain1 scheduling;
! 17 : gain2 scheduling;
! 18 : gain3 scheduling;
! 19 : Kp PID_sin (inpvec(3));
! 20 : Ki PID_sin (inpvec(3));
!
! Local variables
integer*4 i,CtrlStatus
real*8 time,AziAng,y(2),inpvec(3),inpvec_mbc(3)
real*8 pitref(3),meanpitref,meanpitreffilt,kgain(3)
real*8 momcos_filt,momsin_filt,pitref_mbc(3),pitref_mbc_coupled(3)
real*8 omega,omegafilt
real*8 switchfactor,r_switch_filt
! Input
time =array1(1)
AziAng=array1(2) ! in rad [-pi,+pi]
omega =array1(3)
inpvec=array1(3+rev_blade_nn) ! Any user defined signal basically here, in rotating coordinates
pitref=array1(6+rev_blade_nn) ! REMEMBER, rev_blade_no is 1,3,2. DEPENDS ON BLADE NUMBERING AND ROTATION DIRECTION
! in non-rotating coordinates this should be 1,2,3
CtrlStatus=int(array1(10))
! Specify the time for switch on individual pitch
if (CtrlStatus.eq.0 .and. time .gt. time_on) then
switchfactor=1.d0
else
switchfactor=0.d0
endif
! active the ipc when the ctrl switch value from main dll is larger than thr_ratcyclic
if (array1(11).gt.thr_ratcyclic) then
r_switch_filt = 1.d0
else
r_switch_filt = 0.d0
endif
switchfactor=lowpass1orderfilt(deltat,stepno,LP1_status_var,r_switch_filt*switchfactor)
! Increment time step (may actually not be necessary in type2 DLLs)
if (time-time_old.gt.1.d-6) then
deltat=time-time_old
time_old=time
stepno=stepno+1
endif
! Reference pitch angles and its low-pass filter for gain scheduling
meanpitref=(pitref(1)+pitref(2)+pitref(3))/3.d0
meanpitreffilt=lowpass1orderfilt(deltat,stepno,LP1_pit_var,meanpitref)
kgain=1.d0/(1.d0+meanpitreffilt*invkk1+meanpitreffilt**2*invkk2)
! Low-pass filtered rotor speed
omegafilt=lowpass1orderfilt(deltat,stepno,LP1_rpm_var,omega)
NP_cos_var%f0=omegafilt/2.d0/pi
NP_sin_var%f0=omegafilt/2.d0/pi
! Inverse Coleman transform to get non-rotating Multi-Blade-Coordinates,
! but only when the input is in rotating coordinates
if (input_coord.gt.0.d99) then
inpvec_mbc=matmul(InvBmat(AziAng),inpvec)
else
inpvec_mbc=inpvec
endif
! Low-pass filter these asymmetric moments
y=lowpass2orderfilt(deltat,stepno,LP2_cos_var,inpvec_mbc(2))
momcos_filt=y(1)
y=lowpass2orderfilt(deltat,stepno,LP2_sin_var,inpvec_mbc(3))
momsin_filt=y(1)
! Notch filter these low-pass filtered asymmetric moments on the rotor speed
! Seems that for whatever reason this fails to work proper for NP_sin_var??
momcos_filt=notch2orderfilt(deltat,stepno,NP_cos_var,momcos_filt)
momsin_filt=notch2orderfilt(deltat,stepno,NP_sin_var,momsin_filt)
! Set collective pitch to mean and calculate the PID feedback
pitref_mbc(1)=meanpitref
pitref_mbc(2)=PID(stepno,deltat,kgain,PID_cos_var,momcos_filt)
pitref_mbc(3)=PID(stepno,deltat,kgain,PID_sin_var,momsin_filt)
! apply the coupling matrix between q and d elements
pitref_mbc_coupled=matmul(Jinv,pitref_mbc)
! Satisfy hard limits
!if (PID_cos_var%outres.lt.PID_cos_var%outmin) then
! PID_cos_var%outres=PID_cos_var%outmin
!elseif (PID_cos_var%outres.gt.PID_cos_var%outmax) then
! PID_cos_var%outres=PID_cos_var%outmax
!endif
! and max velocity limits
!if (PIDvar%velmax.gt.eps) then
! if ((abs(PIDvar%outres-PIDvar%outres1_old)/dt).gt.PIDvar%velmax) then
! PIDvar%outres=PIDvar%outres1_old+dsign(PIDvar%velmax*dt,PIDvar%outres-PIDvar%outres1_old)
! endif
!endif
! Coleman transform including the lead angle to get the rotating pitch references
! ToDO: Do we need to gain schedule the lead anlge?
array2(rev_blade_nn) = switchfactor*(matmul(Bmat(AziAng+LeadAngle),pitref_mbc_coupled)-pitref)
!array2(rev_blade_nn)=pitref+switchfactor*(matmul(Bmat(AziAng+LeadAngle),pitref_mbc)-pitref)
array2(4:6) =inpvec_mbc(1:3) ! input signal in MBC (non-rotating)
array2(7) =momcos_filt ! LP + Notch filtered input signal inpvec(2)
array2(8) =momsin_filt ! LP + Notch filtered input signal inpvec(3)
array2(9) =pitref_mbc(1) ! mean pitch reference
array2(10:11)=pitref_mbc(2:3) ! PID on LP+Notch filtered input signal(2:3)
array2(12) =switchfactor ! switch factor
array2(13:15)=pitref_mbc_coupled(1:3) ! matmul(Jinv,pitref_mbc)
array2(16:18)=kgain(1:3) ! gain scheduling parameter
!array2(15:17)= matmul(Bmat(AziAng+LeadAngle),pitref_mbc_coupled)
array2(19) = PID_sin_var%outpro ! proportional gain
array2(20) = PID_sin_var%outset ! integral gain
return
end subroutine update_cpc_track
!**************************************************************************************************
end module cyclic_pitch_controller_mod
module cyclic_pitch_controller_fcns_mod
use misc_mod
use user_defined_types
!use global_variables
use misc_mod
implicit none
! Constant matrices
integer*4 rev_blade_no(3)
integer*4 rev_blade_no(3), rev_blade_nn(3)
data rev_blade_no /1,3,2/
data rev_blade_nn /1,2,3/
real*8 B0mat(3,3),Bcmat(3,3),Bsmat(3,3)
real*8 B0inv(3,3),Bcinv(3,3),Bsinv(3,3)
real*8 Jinv(3,3)
! Cyclic_pitch_controller parameters:
real*8 :: LeadAngle = 0.d0
real*8 invkk1,invkk2
real*8 :: thr_ratcyclic = 0.99d0
type(Tlowpass2order) LP2_cos_var
type(Tpidvar) PID_cos_var
type(Tlowpass2order) LP2_sin_var
type(Tpidvar) PID_sin_var
type(Tfirstordervar) LP1_pit_var
type(Tfirstordervar) LP1_rpm_var
type(Tnotch2order) NP_cos_var
type(Tnotch2order) NP_sin_var
type(Tfirstordervar) LP1_status_var
real(8) :: time_on = 50.0
integer*4 :: input_coord=1 ! 1 for rotating coordinates, 0 for non-rotating
type(Tlowpass2order), save :: LP2_cos_var
type(Tpidvar), save :: PID_cos_var
type(Tlowpass2order), save :: LP2_sin_var
type(Tpidvar), save :: PID_sin_var
type(Tfirstordervar), save :: LP1_pit_var
type(Tfirstordervar), save :: LP1_rpm_var
type(Tnotch2order), save :: NP_cos_var
type(Tnotch2order), save :: NP_sin_var
type(Tfirstordervar), save :: LP1_status_var
real(8) :: time_on = 0.0
! Simulations variables
integer*4::stepno=0
real*8::time_old=0.d0
......@@ -59,6 +64,11 @@ Bsinv(2,3)= sqrt(3.d0)/3.d0
Bsinv(3,1)= 2.d0/3.d0
Bsinv(3,2)=-1.d0/3.d0
Bsinv(3,3)=-1.d0/3.d0
! init Jacobian coupling matrix to diagonal (no coupling)
Jinv=0.d0
Jinv(1,1)=1.d0
Jinv(2,2)=1.d0
Jinv(3,3)=1.d0
return
end subroutine setup_mbc_matrices
!**************************************************************************************************
......@@ -103,24 +113,28 @@ if (stepno.gt.PIDvar%stepno1) then
PIDvar%error1_old=PIDvar%error1
endif
! Update the integral term
PIDvar%outset=PIDvar%outset1_old+0.5d0*(error+PIDvar%error1)*Kgain(2)*PIDvar%Kint*dt
PIDvar%outset=PIDvar%outset1_old+0.5d0*(error+PIDvar%error1_old)*Kgain(2)*PIDvar%Kint*dt
! Update proportional term
PIDvar%outpro=Kgain(1)*PIDvar%Kpro*0.5d0*(error+PIDvar%error1)
PIDvar%outpro=Kgain(1)*PIDvar%Kpro*0.5d0*(error+PIDvar%error1_old)
! Update differential term
PIDvar%outdif=Kgain(3)*PIDvar%Kdif*(error-PIDvar%error1_old)/dt
! Sum to up
PIDvar%outres=PIDvar%outset+PIDvar%outpro+PIDvar%outdif
! doesn't work f you have not a pitch signal but any general error signal
! Satisfy hard limits
if (PIDvar%outres.lt.PIDvar%outmin) then
PIDvar%outres=PIDvar%outmin
elseif (PIDvar%outres.gt.PIDvar%outmax) then
PIDvar%outres=PIDvar%outmax
endif
!if (PIDvar%outres.lt.PIDvar%outmin) then
! PIDvar%outres=PIDvar%outmin
!elseif (PIDvar%outres.gt.PIDvar%outmax) then
! PIDvar%outres=PIDvar%outmax
!endif
! doesn't work f you have not a pitch signal but any general error signal
! Satisfy max velocity
if (PIDvar%velmax.gt.eps) then
if ((abs(PIDvar%outres-PIDvar%outres1_old)/dt).gt.PIDvar%velmax) &
PIDvar%outres=PIDvar%outres1_old+dsign(PIDvar%velmax*dt,PIDvar%outres-PIDvar%outres1_old)
endif
!if (PIDvar%velmax.gt.eps) then
! if ((abs(PIDvar%outres-PIDvar%outres1_old)/dt).gt.PIDvar%velmax) then
! PIDvar%outres=PIDvar%outres1_old+dsign(PIDvar%velmax*dt,PIDvar%outres-PIDvar%outres1_old)
! endif
!endif
! Anti-windup on integral term and save results
PIDvar%outset1=PIDvar%outres-PIDvar%outpro-PIDvar%outdif
PIDvar%outres1=PIDvar%outres
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment