This is the only thing about this code that looks off, maybe it is just a matter of language, but in this context "trimmedAttitudeSetpoint" doesn't sound good to me.