The pitch reference was seen to not be stored when the controller goes from status=-1 to status=0, which resulted in a large pitch step. This PR was found to solve the problem.