Add wind compensation and i_term init

Untested, give a try tomorrow.