madgwick code cleaup