mirror of https://github.com/ArduPilot/ardupilot
Copter: reset ekf height if arming before home set
This commit is contained in:
parent
9f17fc17ab
commit
14cf9b1621
|
@ -153,8 +153,11 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
|
|||
|
||||
initial_armed_bearing = ahrs.yaw_sensor;
|
||||
|
||||
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)
|
||||
if (ap.home_state == HOME_SET_NOT_LOCKED) {
|
||||
set_home_to_current_location();
|
||||
}
|
||||
calc_distance_and_bearing();
|
||||
|
|
Loading…
Reference in New Issue