IMPORTANT baro and sonar fixes