ForceSetPoint.tex

\documentclass[11pt]{article}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Preamble
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\input{tPreambule.tex}
\usepackage{fullpage}
% \usepackage[pass, showframe]{geometry}
\setlength{\itemsep}{-10em}

\newcommand{\question}[1]{{\color{red}{#1}}}
\begin{document}
\title{Development plan for the linearization about a rotational speed set-point in OpenFAST}
\author{E. Branlard, J.Jonkman}
\maketitle
% \tableofcontents 
% \newpage

\section*{Introduction}
This document describes the implementation of an algorithm to perform %close-loop
linearization about a rotational set-point in \textit{OpenFAST}. Currently, the user needs to manually adjust operating parameters until the desired rotational speed is reached, or until the transients are eliminated, before the linearization can be performed. This procedure is here automatized using a simple iterative algorithm. The method supports simulations with zero rotational speed, a fixed rotational speed (generator off), or simulations with a variable speed (generator on). In the later case, the algorithm presented adjusts one of the following operating parameter to reach the target rotational speed: the blade pitch angle, the generator torque or the neutral yaw position of the turbine. A proportional gain on the rotational speed error is used to adjust the parameters. To speed-up the convergence, the damping may be artificially increased in the iteration step. At the end of the iteration algorithm, the transients have dissipated, the rotor is at the target rotational speed and a steady or periodic operating point is reached. The linearization is then performed for one rotor revolution (if applicable) without the previously added damping.
The algorithm is such that it only involves modifications of the \textit{OpenFAST} glue-code and of \tt{ServoDyn}.


% \clearpage
\section{OpenFAST implementation}

\subsection{Basic workflow and changes to the code}
The basic workflow is listed below. More details follow in subsequent sections.
\begin{itemize}\tightlist
    \item New parameters are read from the input files (given in \autoref{sec:newinputs})
    \item If the parameter \tt{CalcSteady} is false, the \textit{OpenFAST} linearization process will happen as before (at \tt{LinTimes})
    \item 
      If \tt{CalcSteady} is true, \textit{OpenFAST} will iterate and update a controller parameter
      until the rotational speed matches the parameter \tt{RotSpeed} given in \textit{ElastoDyn}'s input file, and then perform the linearization at this periodic operating point.
%      to find the controller values required to get a periodic operating point before performing the linearization. 
%       A linearization will then proceed at this periodic operating point. 
     A different controller variable is adjusted in the iterative step based on the value of the parameter \tt{TrimCase}: the neutral yaw \tt{YawNeut}, the generator torque \tt{GenTrq} or the blade pitch \tt{BlPitch}. 
     The procedure follows the steps below.
%      The process is described below.
    \item After the initialization of \textit{ElastoDyn}, additional initialization inputs are passed to \textit{ServoDyn} for its initialization:  the glue-code inputs \tt{CalcSteady}, \tt{TrimCase} and \tt{TrimGain} and the reference rotational speed \tt{RotSpeedRef}. These initialization inputs will be used by \textit{ServoDyn} to adjust one of the controller parameter based on the current rotational speed error. If the reference rotation speed is $0$ or if the generator degree of freedom is off, no controller trimming is required. 
    \item \textit{ServoDyn} is initialized. The discrete-time state \tt{CtrlOffset} is added to the module to keep track of the controller parameter offset.
    \item The glue-code starts a special time stepping loop where a convergence criteria is checked upon after each revolution, or time step if the rotational speed is zero.  The time stepping loop has the following characteristics:
    \begin{itemize}\tightlist
    \item At each time step, if the controller trimming is active, \textit{ServoDyn} updates its controller offset state (\tt{CtrlOffset}) based on the proportional gain \tt{TrimGain} and the error in rotational speed. This offset is added to the controller commands of \textit{ServoDyn} and will hence have an influence on \textit{ElastoDyn}. 
    \item At each time step, the glue-code adds additional damping, via external forces, to the structure of the modules \textit{ElastoDyn} and \textit{BeamDyn}.
    \item At given azimuthal positions (defined by \tt{NAzimStep}), the glue-code computes the relative difference between the output vector of the current revolution and the previous revolution. If the rotational speed is zero, the difference is computed between two successive time steps and the number of azimuthal step is effectively 1.
    \item When this difference is below the tolerance \tt{TrimTol} for all the reference azimuthal positions, the simulation has reached a periodic steady state, which also implies that the controller offset parameter has also converged and the rotational speed of the rotor matches the requested set-point. The time-stepping is stopped
    \end{itemize}
    \item  The linearization is performed for one rotor revolution (if applicable) at steps of \tt{NAzimStep}. The operating point is at the requested rotational speed and it uses the controller offset obtained by the iteration procedure above.
\end{itemize}


The following sections describe the changes needed to the code:
\begin{itemize}
    \item \autoref{sec:newinputs}: new input parameters to be added to the \textit{OpenFAST} glue-code
    \item \autoref{sec:newEDinitoutputs}: changes to \textit{ElastoDyn} to return needed information
    \item \autoref{sec:gluecodeiterative}: iterative procedure of the glue-code to ensure a periodic steady state is reached
    \item \autoref{sec:changesservodyn}: changes to \textit{ServoDyn} to compute the controller parameter offset
    \item \autoref{sec:damping}: glue-code procedure to increase the damping and accelerate the convergence
\end{itemize}


\subsection{New glue code inputs}
\label{sec:newinputs}
The following input are added to the main \textit{OpenFAST} input file (\tt{.fst} file):
%     \fortran
% \begin{lstlisting}
% CalcSteady - Linearize about periodic operating point (flag)
% TrimCase   - Controller parameter to be trimmed {1=yaw; 2=torque; 3=pitch} [1,2,3] [unused if CalcSteady=False]}
% TrimTol    - Tolerance for the rotational speed convergence [>=0] [used only when ClacSteady=True]
% Lin_Kom    - Proportional gain for the rotational speed error (rad/(rad/s) or Nm/(rad/s)) [>0] [used only when CalcSteady=True]
% Twr_Kdmp   - Damping factor for the tower (N/(m/s)) [>=0] [used only when CalcSteady=True]
% Bld_Kdmp   - Damping factor for the blade (N/(m/s)) [>=0] [used only when CalcSteady=True]
% NAzimStep  - Number of equally-spaced azimuth steps in periodic linearized model (-) [>=1]
% \end{lstlisting}
\begin{itemize}\tightlist
    \item \tt{CalcSteady - Calculate a steady-state periodic operating point before linearization (-) (switch)}
    \item \tt{TrimCase - Controller parameter to be trimmed \{1:yaw; 2:torque; 3:pitch\} [used only when CalcSteady=True]}
    \item \tt{TrimTol - Tolerance for the rotational speed convergence [>eps] [used only when CalcSteady=True] }
    \item \tt{TrimGain - Proportional gain for the rotational speed error (rad/(rad/s) or Nm/(rad/s)) [>0] [used only when CalcSteady=True] }
    \item \tt{Twr\_Kdmp - Damping factor for the tower (N/(m/s)) [>=0] [used only when CalcSteady=True] }
    \item \tt{Bld\_Kdmp - Damping factor for the blade (N/(m/s)) [>=0] [used only when CalcSteady=True] }
%    \item \tt{NAzimStep  - Number of equally-spaced azimuth steps in periodic linearized model (-) [>=1] }
\end{itemize}

Note: \tt{NAzimStep} is equivalent to \tt{NLinTimes}, which is already in the \textit{OpenFAST} input file. Also note that in implementation,
\tt{TrimTol} must be larger than epsilon. \question{or maybe we should require it to be even larger?}

\question{Do we want to adjust more damping values?}
Linearization inputs are read in \tt{Fast\_Subs.f90}, routine \tt{FAST\_ReadPrimaryFile} about line 2273. They are returned in the structure named \tt{p} or \tt{p\_FAST} of type \tt{FAST\_ParameterType}. The parameters above need to be added to the \tt{FAST\_Registry.txt} file as \tt{FAST\_ParameterType}.
% \clearpage





\subsection{New initialization outputs from \textit{ElastoDyn}}
\label{sec:newEDinitoutputs}

\paragraph{New registry types}
The initialization outputs \tt{RotSpeed} and \tt{isFixed\_GenDOF} are added: 
\fortran
\begin{lstlisting}
InitOutputType  ReKi     RotSpeed       - - - "Initial or fixed rotor speed" rad/s
InitOutputType  Logical  isFixed_GenDOF - - - "Whether the generator DOF is fixed (true) or free (false)" -
\end{lstlisting}

These variables are sent to the initialization routine of the \textit{ServoDyn} module.

Note: We can't use the \textit{ElastoDyn} output type to initialize \textit{ServoDyn} because the 
initialization routines do not necessarily calculate output variables (the values defining the output meshes are the only
values required to be initialized in the output type). The only exception to this is when \textit{BeamDyn} is used.


\subsection{Main glue-code procedure}
\label{sec:gluecodeiterative}


\paragraph{Main program}
\begin{itemize}\tightlist
    \item If \tt{CalcSteady} is false, set \tt{Twr\_Kdmp} and \tt{Bld\_Kdmp} to $0$ and proceed as usual
    \item If \tt{CalcSteady} is true, follow the iterative procedure below
\end{itemize}


\paragraph{Iterative procedure}
% \paragraph{Notations} 
The subscript $p$ is used to refer to the \textit{previous} time step, the subscript $c$ is used for the \textit{current} time step, the subscript $0$ is used to refer to the target azimuthal positions.
% 
The azimuthal angle $\psi$ 
% and the rotor speed $\Omega$ 
at a given time step are taken from the outputs of \textit{ElastoDyn}: 
\begin{align}
    \psi = \tt{ED\%Output(1)\%LSSTipPxa}
\end{align}


% and \tt{ED\%y\%RotSpeed} respectively.
% 
% 
% 
% \paragraph{Allocations} \ 
The following storage variables are used by the iterative algorithm:
\begin{table}[!h]\centering
\begin{tabular}{ccp{10cm}}
\textbf{Variable} & \textbf{Dimensions}& \textbf{Description}\\
\hline
$n_{rot}$ & $1 \times 1$ & Number of full rotor revolutions completed; if the reference rotational speed is zero, each time step is considered a full revolution.\\
$j$ & $1 \times 1$ & Index into target azimuth positions, $j \in [1,\tt{NAzimStep}]$\\
$n_y$ & $1 \times 1$ & Total number of outputs included in the linearization analysis of all modules, excluding any \tt{WriteOutput} and extended output values\\
$\v{\psi}_0$ & $1\times \tt{NAzimStep}$ & Target azimuthal positions, $\v{\psi}_0[j] \in [0, 2\pi)$\\
$\v{y}_c$ & $1\times n_y$ & Output vector (from all modules) at current time step (used for interpolation)\\
$\v{y}_p$ & $1\times n_y$ & Output vector (from all modules) at previous time step (used for interpolation)\\
$\v{y}_0$ & $1\times n_y$ & Output vector at a target azimuthal position, interpolated using $\v{y}_c$ and $\v{y}_p$\\
$\m{Y}_0$ & $n_{y}\times \tt{NAzimStep}$ & Output vector interpolated at each target azimuthal position $\psi_0[j]$ (stored from previous revolution)\\
$\v{\epsilon}_y$ & $1 \times \tt{NAzimStep}$ & Relative error in the output vector between two revolutions at the same target azimuthal position \\
\hline
\end{tabular}
\end{table}


The following steps make up the iterative procedure:
\begin{itemize}\tightlist
%     \item Allocate the matrix $\m{Y}_0$ of dimension $n_{y}\times \tt{NAzimStep}$ to store the output vector from all the modules at each target azimuthal position $\v{\psi}_0$, where $n_{y}$ is the number of outputs from all the modules. 
%     \item Allocate $\v{p}_p$, the vector of output from the previous time step, and set it to the current output vector.
% \item 
%     \begin{align}
%          \v{y}_p =\operatorname{NaN}(1,n_y)
%     \end{align}

    \item If the generator degree of freedom is off or if the rotational speed is zero, \tt{TrimCase} is set to $0$ so that the trimming is cancelled. This step is done prior to the initialization of \textit{ServoDyn} presented in \autoref{sec:changesservodyn}.
    \item If the reference rotational speed is $0$, ensure that \tt{NAzimStep}=1. The number of rotations is then understood as the number of time steps.
    \item Initialize the number of full rotor revolutions and the index of target azimuthal positions:
        \begin{align}
            n_\text{rot} = 0
         , \qquad
         j = 1
%          , \qquad
%       \epsilon_\Omega=0
%          , \qquad
%          \psi_p = \psi_\text{init}
%               %\label{eq:}
        \end{align}
    \item Perform time stepping until $\tt{TMax}$ (the time loop will be stopped before $\tt{TMax}$ if the convergence criteria is met).
%       The output vector is stored for each target azimuthal position $\v{\psi}_0$. 
      For each time step:
        \begin{itemize}\tightlist
            \item Call the time step integration routine \tt{FAST\_Solution\_T}. This routine applies an increased damping (based on \tt{Twr\_Kdmp} and \tt{Bld\_Kdmp}, see \autoref{sec:damping})
   If the rotational speed is non-zero and if the generator is on, \textit{ServoDyn} applies an offset to the controller variables (based on $\epsilon_\Omega$, see \autoref{sec:changesservodyn}).
            \item Store the current azimuthal angle and output vector: $\psi_{c}$ and $\v{y}_c$
%             \item Store the current output vector from all the modules $\v{y}_\text{c}$
            \item If $t=0$ (or first time step):
                \begin{itemize}\tightlist
%                     \item Set the reference rotor speed as $\Omega_\text{ref}=\Omega_c$
%                     \item If $\Omega_\text{ref}=0$, exit the trimming algorithm
                    \item Set the initial azimuthal position as $\psi_\text{init}=\psi_c$
                        The azimuthal angle is stored as a number in the interval $[0;\, 2\pi )$ ($2\pi$ excluded), i.e. $\psi_c=\operatorname{mod}(\psi, 2\pi)$, implemented as $\psi_c$=\tt{Zero2TwoPi($\psi$)}.
                    \item Set the vector of target azimuthal positions $\v{\psi}_0$ (also in $[0;\,2\pi)$):
                            \begin{align}
                                 k=1..\tt{NAzimStep}
                                 ,\quad
                                 \v{\psi}_0[k] = \operatorname{mod}(\psi_\text{init} +  (k-1) \Delta\psi, 2\pi), \quad \Delta\psi\eqdef \frac{2\pi}{\tt{NAzimStep}}
                            \end{align}

                    \item Set $\v{y}_p=\v{y}_c$ and $\psi_p=\psi_c$
                \end{itemize}
%             \item Compute the error in rotor speed $\epsilon_\Omega$, the difference between the target speed and the current rotor speed:
%                 \begin{align}
%                     \epsilon_\Omega ={\Omega_c - \Omega_\text{ref}}
%                 \end{align}
%                 This error will be used at the next time step to correct the controller parameter

            \item If $t >0$ and $(\psi_c -\psi_p) \geq \Delta\psi$, return an error: the rotor is spinning too fast; the time step or \tt{NazimStep} are too large.

            \item If $\psi_c \geq \psi_0[j]$ (take care with the $2 \pi$ boundary)
                \begin{itemize}
                    \item Interpolate the output vector to the target azimuthal position $\psi_0[j]$ using the current output values $\v{y}_c$ and the previous ones $\v{y}_p$:
                        \begin{align}
                            \text{if $t=0$ or $\Omega_\text{ref}=0$} , \ 
                            \v{y}_0 = \v{y}_c,
                            \quad \text{else}\quad 
                            \v{y}_0 = \v{y}_p + (\v{y}_c-\v{y}_p)\frac{\psi_0[j]-\psi_p}{\psi_c-\psi_p}   %\label{eq:}
                        \end{align}
                        Note: outputs that are 3D rotations should be transformed to logarithmic maps.
                        Note: special care is needed if angles are close to 0 or $2\pi$, in which case they should be taken between $-\pi$ and $\pi$.
                        Note: The output interpolations will be performed by the auto-generated routines in the FAST Registry. I am implementing this to include using the extrap-interp order
                              used in the FAST input file. In the case that the rotational speed is zero, the extrap-interp order for the CalcSteady algorithm is changed to 0 
                              so it will use the current value. I also modified the FAST Registry and NWTC\_Library to handle interpolation and extrapolation of angles. \\

                    \item If $n_\text{rot}>0$, compute the mean squared relative error of the output vector at the azimuthal position $\psi_0[j]$ between the current revolution and the previous one:
                        \begin{align}
                            \epsilon_y^2[j] %=\rVert\boldsymbol{\epsilon_y}\lVert^2
                            = \frac{1}{n_y} \sum_i \left(\frac{y_0[i]-Y_0[i,j]}{y_\text{ref}[i]}  \right)^2
    %                     ,\qquad \epsilon_{\dot{q}}^2 = \rVert (\boldsymbol{\dot{q}}-\boldsymbol{\dot{q}_0}). \boldsymbol{w}^t\lVert^2
                        \end{align}
                        The reference value $y_\text{ref}$ is defined in \autoref{eq:yref}.\\
                        Note: if the variable $y_0[i]$ is in radian or degree, the difference of the variable should be taken between $-\pi$ and $\pi$, implemented using \tt{MPi2Pi}. \\
                    \item Store the interpolated value
                        \begin{align}
                            \m{Y}_0[:,j] = \v{y}_0
                        \end{align}
                    \item Increment $j$%: $j\leftarrow j+1$
                \end{itemize}

            \item Set the current values as previous values for the next time step
                \begin{align}
                   \psi_p \leftarrow \psi_c
                   ,\qquad
                   \v{y}_p \leftarrow \v{y}_c
                \end{align}

            \item If $j>\tt{NAzimStep}$: 
                    \begin{itemize}
                        \item Increment $n_\text{rot}$
                        \item Check convergence over all azimuthal positions: $\epsilon^2_y[k]<\tt{TrimTol}$ for all $k$
                        \item If converged, exit the time loop
                        \item Otherwise, compute a reference value for each of the index of the output vector, based on the maximum and minimum values taken over one rotor revolution.
                \begin{align}
                    y_{\text{ref}}[i] = \left| \max(Y_0[:,i]) - \min(Y_0[:,i]) \right|
                \quad\text{if }\
                y_\text{ref}[i]>10^{-6}
                ,\quad\text{else }\
                y_\text{ref}[i]=1
                \label{eq:yref}
                \end{align}                
                \begin{align}
                \text{For angles,} \quad y_{\text{ref}}[i] = \min(\pi,y_{\text{ref}}[i])
                \end{align}                
                \item Set $j=1$ and continue the time stepping.
            \end{itemize}

    \end{itemize}
    \item If the time loop run up to \tt{TMax}, return an error, otherwise perform the linearization step below
\end{itemize}
\paragraph{Linearization}
\begin{itemize}
% \item The values \tt{Twr\_Kdmp} and \tt{Bld\_Kdmp} are set to 0 % ignoring this due to added complication; damping should have almost no effect on the converged solution
% \item The controller offset is kept to its value $p_\text{off}$
\item The standard linearization procedure takes place
\end{itemize}




% --------------------------------------------------------------------------------}
% --- SERVODYN 
% --------------------------------------------------------------------------------{
\subsection{Changes in \textit{ServoDyn}}
\label{sec:changesservodyn}
In the updated implementation, \textit{ServoDyn} has the possibility to modify some of its outputs based on offset that is updated internally as a discrete state.

\paragraph{New registry types}
The inputs \tt{RotSpeedRef}, \tt{TrimCase}, \tt{TrimGain} are added: 
\fortran
\begin{lstlisting}
InitInputType	IntKi	TrimCase	-	-	-	"Controller parameter to be trimmed"	-
InitInputType	ReKi	TrimGain	-	-	-	"Proportional gain on rotational speed error"	-
InitInputType	ReKi	RotSpeedRef	-	-	-	"Reference rotational speed"	rad/s
\end{lstlisting}
These inputs should also be added as \tt{ParameterType} in the registry file.
%          , \qquad
The discrete state \tt{CtrlOffset} (also noted $x_\text{off}$) is added: 
%       \epsilon_\Omega=0
\begin{lstlisting}
DiscreteStateType	ReKi	CtrlOffset	-	-	-	"Controller offset parameter"	-
\end{lstlisting}


\paragraph{Glue-code transfer before the init routine}
The controller trimming option of \textit{ServoDyn} requires additional parameters from the glue-code and \textit{ElastoDyn}. These parameters need to be transferred via the \tt{SrvD\_InitInputType} structure.
Currently, these transfer occur in the routine \tt{FAST\_InitializeAll} of \tt{FAST\_Subs.f90}. 
The following transfer is added: 
\fortran
\begin{lstlisting}
InitInData_SrvD%TrimCase    = p_FAST%TrimCase
InitInData_SrvD%TrimGain    = p_FAST%TrimGain
InitInData_SrvD%RotSpeedRef = InitOutData_ED%RotSpeed
\end{lstlisting}
If the generator degree of freedom is off or if the rotational speed is zero, \tt{TrimCase} is set to $0$ so that the trimming is canceled.
Note that this is done in the glue code prior to calling \textit{ServoDyn} (because \textit{ServoDyn} does not know any DOF information from
the structural code).


\paragraph{Initialization routine \tt{Srvd\_Init}}
The added variables from \tt{InitInputType} are copied to the \tt{ParameterType} variables.
The state variable \tt{CtrlOffset} is initialized to 0.
If the parameter \tt{TrimGain} is not strictly positive, an error is thrown.


\paragraph{Update state routine \tt{Srvd\_UpdateDiscState}}
A simple proportional gain on the rotational speed error is used to correct the control parameters.
The error in rotor speed $\epsilon_\Omega$ is the difference between the target speed and the current rotor speed:
        \begin{align}
            \epsilon_\Omega ={\Omega_c - \Omega_\text{ref}}
        \end{align}
The current rotor speed is $\Omega_c=\tt{u\%RotSpeed}$ where \tt{u} is defined at time \tt{t} (i.e., not \tt{t+1}).
        The offset on the controller variable is computed using 
        the speed error and a proportional gain $k_p>0$. The offset is computed as:
        \begin{align}
            x_\text{off} = x_\text{off} + s\, k_p\, \epsilon_\Omega \label{eq:xoffpitch}
        \end{align}
The variable $s$ above accounts for possible sign adjustments. 
The offset is such that it will converge to a constant value as $\epsilon_\Omega$ tends to 0.
When \tt{TrimCase}=1, $x_\text{off}$ is the yaw angle offset (in rad). 
When \tt{TrimCase}=2, $x_\text{off}$ is the generator torque offset (in Nm). 
When \tt{TrimCase}=3, $x_\text{off}$ is the pitch angle offset (in rad). 
% The offset 
For the pitch and generator torque, $s=1$. Indeed, when the rotational speed is faster than $\Omega_\text{ref}$ ($\epsilon_\Omega>0)$, the  pitch or generator torque needs to be increased to lower the rotational speed. The opposite holds when the rotor spins slower than $\Omega_\text{ref}$.
%     For the pitch and 
% The main control variable for the yaw is the yaw moment. The yaw moment is computed as $-k_\text{yaw} (\theta_\text{ED} - \theta_0)$.
On the other hand, when $\epsilon_\Omega>0$, the yaw angle needs to be increased if the yaw angle is positive, or decreased if this angle is negative, in order to decrease the rotational speed. For this case, $s=\operatorname{sign}( \theta_{\text{yaw},0}+x_\text{off})$, where $\theta_{\text{yaw},0}$ is the neutral yaw angle defined by \tt{p\%YawNeut}.
Another subtlety arises for the yaw case, since the main variable of \textit{ServoDyn} is actually the yaw moment. Yet, it is more convenient to manipulate an offset on the yaw angle since the offset sign depends on the yaw angle. 
This issue will be addressed in the next paragraph.
The update of the discrete state is implemented as follows:
\begin{lstlisting}
if ((TrimCase==2).or.(TrimCase==3)) then
    xd%CtrlOffset += (u%RotSpeed - p%RotSpeedRef) * p%TrimGain 
else if ((TrimCase==1) then
    xd%CtrlOffset += (u%RotSpeed - p%RotSpeedRef) * sign(p%TrimGain, p%YawNeut + xd%CtrlOffset)
else
    xd%CtrlOffset = 0
endif
\end{lstlisting}




\paragraph{Output routine \tt{SrvD\_CalcOutput}}
The output variables of \textit{ServoDyn} are directly modified using the offset \tt{CtrlOffset}.
As mentioned in the previous paragraph, in the yaw case, the main variable output by \textit{ServoDyn} is the yaw moment and not the yaw angle. The part of the yaw moment that depends on the yaw angle is computed as:
\begin{align}
    Q_\text{yaw}= - k_\text{yaw} ( \theta_\text{yaw,ED} - \theta_{\text{yaw},0})
\end{align}
Hence, if $\theta_{\text{yaw},0}$ is replaced by $\theta_{\text{yaw},0}+x_\text{off}$, it is seen that the yaw moment is given the offset $k_\text{yaw} x_\text{off}$.
% 
% 
For the implementation, the control outputs are trimmed just after their computation within the  \tt{SrvD\_CalcOutput} routine, that is after calling \tt{Pitch\_CalcOutput}, \tt{Torque\_CalcOutput} and \tt{Yaw\_CalcOutput}, as follows:
\begin{lstlisting}
if (TrimCase==1) then
    y%YawMom     = y%YawMom      + xd%CtrlOffset * p%YawSpr
else if (TrimCase==2)
    y%GenTrq     = y%GenTrq      + xd%CtrlOffset
else if (TrimCase==3)
    y%BlPitchCom = y%BlPitchCom  + xd%CtrlOffset
else
    ! do nothing
endif
\end{lstlisting}
By doing the update in \tt{Svrd\_CalcOutput}, it is ensured that the operating point will be about the proper conditions. Indeed, in \tt{Svrd\_GetOP}, the operating point variables are set from the outputs directly:
\begin{lstlisting}
y_op(Indx_Y_BlPitchCom) = y%BlPitchCom
y_op(Indx_Y_YawMom)     = y%YawMom 
y_op(Indx_Y_GenTrq)     = y%GenTrq
\end{lstlisting}
It is important to note that the routines \tt{CalculateStandardYaw} and \tt{CalcuateTorque} returns values without offset. These routines are for instance called by  \tt{Yaw\_UpdateStates}  and \tt{Torque\_UpdateStates}.
% \weird{Yaw\_CalcOutput, calls \tt{CalculateStandardYaw}, also called by \tt{Yaw\_UpdateStates}}
% \weird{Torque\_CalcOutput, calls \tt{CalcuateTorque}, also called by \tt{Torque\_UpdateStates}}

\paragraph{Linearization routine \tt{SrvD\_JacobianPInput}}
This routine computes the partial derivatives of outputs with respect to inputs. The \textit{ServoDyn} inputs in the linearization analysis
include only \tt{Yaw}, \tt{YawRate}, and \tt{HSS\_Spd}. The modified output equations in \tt{SrvD\_CalcOutput} are not functions of 
any of these variables, thus the partial derivatives are unchanged.

% 
% 
% 
% 
% 
% Depending on \tt{TrimCase}, the variable $p$ is either the yaw degree of freedom, the generator torque or the pitch angle. 
% 






% --------------------------------------------------------------------------------}
% --- DAMPING 
% --------------------------------------------------------------------------------{
\subsection{Additional glue-code procedure to increase the damping}


\label{sec:damping}
Artificial damping forces are added to the external forces applied on the structure.
For now, the extra damping is only applied to \textit{ElastoDyn} and \textit{BeamDyn}.
% 
The extra damping force is set to be proportional to the velocity of each node of the structure. The proportionality constants \tt{Twr\_Kdmp} and \tt{Bld\_Kdmp} are used respectively for nodes on the tower or the blade. 
% For instance, the $j^{th}$ node of blade $i_B$ in \textit{ElastoDyn} is found in the following mesh $\text{ED}\%u\%\text{BladeLn2Mesh}[i_B]$.
% Writing this node $u_\text{B}[j]$, the additional forces is computed as:
% 
% This force is computed at each node of \textit{ElastoDyn}, based on the velocities of this node:
% Writing
% \text{ED}\%y\%Mesh
In general, the force $\v{F}$ on a given node of velocity $\v{v}$ is updated as follows:
\begin{align}
    \v{F} \leftarrow  \v{F}  -  k_\text{dmp}\  \v{v}
\end{align}
To avoid damping the rotation of the blade, the following is applied for nodes along the blade:
\begin{align}
    \v{F} \leftarrow  \v{F}  -  k_\text{dmp}\  (\v{v}-\v{\Omega}_c\times \v{r})
\end{align}
where $\v{r}$ is the instantaneous position vector of a blade node.
The forces are usually found as inputs of a module while the kinematics are found in the outputs. 
% 
The additional damping forces can be implemented in the procedure \tt{ED\_InputSolve} of the glue-code.
% % 
For \textit{ElastoDyn}, the update of the forces 
\begin{lstlisting}
u%TowerPtLoads%Force(1:3,J) -= Twr_Kdmp * y%TowerLn2Mesh%TranslationVel(1:3,J)
u%BladePtLoads(K)%Force(1:3,J) -= Bld_Kdmp * ( y%BladeLn2Mesh(K)%TranslationVel(1:3,J) - Vrot)
\end{lstlisting}
where $K$ is the blade number and $J$ is the node index (along the tower or blade, looping til \tt{NNodes}), $u$ and $y$ are the module input and outputs, and $\tt{Vrot}=\v{\Omega_c}\times\v{r}$
is the velocity due to the rotation of the blade node, computed using \autoref{eq:Vrot}.

For \textit{BeamDyn}, the additional damping is added as follows:
\begin{lstlisting}
u(k)%DistrLoad%Force(1:3,J) -= Bld_Kdmp * (y(k)%BladeMotion%TranslationVel(1:3,J) - Vrot)
\end{lstlisting}

The variable $\tt{Vrot}=\v{\Omega_c}\times\v{r}$ is computed for each blade node using
\begin{align}
    \v{\Omega}_c  &= \Omega_c \cdot \tt{y\%HubPtMotion\%Orientation(1,:,1)}\nonumber\\
    \v{r} &= \v{r}_\text{node} - \v{r}_\text{hub} \label{eq:Vrot}  \\
    \v{r}_\text{hub} &= \tt{y\%HubPtMotion\%Position(1:3,1)} + \tt{y\%HubPtMotion\%TranslationDisp(1:3,1)}  \nonumber \\
    \v{r}_\text{node} &= \tt{y\%BladeLn2Mesh(K)\%Position(1:3,J)} + \tt{y\%BladeLn2Mesh(K)\%TranslationDisp(1:3,J)}  \nonumber 
\end{align}
where $\v{r}_\text{node}$ is defined for \textit{ElastoDyn} above. It is defined in \textit{BeamDyn} as:
\begin{align}
    \v{r}_\text{node} &= \tt{y(k)\%BldMotion\%Position(1:3,J)} + \tt{y(k)\%BldMotion(K)\%TranslationDisp(1:3,J)}  \nonumber 
\end{align}
assuming that \tt{y(k)\%BldMotion} is a sibling mesh of \tt{u(k)\%DistrLoad}. In the case that these meshes are not siblings, 
\tt{MeshMapData\%y\_BD\_BldMotion\_4Loads(k)} should be used in place of \tt{y(k)\%BldMotion}.


% \input{APPENDIX.tex}

% --------------------------------------------------------------------------------}
% --- BIBLIO 
% --------------------------------------------------------------------------------{
%\bibliographystyle{unsrt}
%\bibliography{Bibliography}

\end{document}