In fact I would prefer removing the trimedStabilizationDesired variable entirely since it appears from formatting like a UAVO (I had to go check it wasn't).  Instead since you only use it right here just make the calculation of rpy_desired account for it.