From 14cf9b162120fdc1c487a4faacb8b0fa4ef44c17 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 24 Jun 2015 15:54:26 +0900 Subject: [PATCH] Copter: reset ekf height if arming before home set --- ArduCopter/motors.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 1f24e73085..301c5d7411 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -153,8 +153,11 @@ bool Copter::init_arm_motors(bool arming_from_gcs) initial_armed_bearing = ahrs.yaw_sensor; - // Reset home position if it has already been set before (but not locked) - if (ap.home_state == HOME_SET_NOT_LOCKED) { + if (ap.home_state == HOME_UNSET) { + // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home) + ahrs.get_NavEKF().resetHeightDatum(); + } else if (ap.home_state == HOME_SET_NOT_LOCKED) { + // Reset home position if it has already been set before (but not locked) set_home_to_current_location(); } calc_distance_and_bearing();