From 8873e92c7c95cfaf0dff89266bbc3119b616bfd3 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Tue, 19 Oct 2021 11:20:16 +0200
Subject: [PATCH] ekf: force fallback to baro if GPS is stopped while in GPS
 height mode

Otherwise, no height aiding source is used
---
 src/modules/ekf2/EKF/ekf_helper.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
index 82518e2eef..c522304e38 100644
--- a/src/modules/ekf2/EKF/ekf_helper.cpp
+++ b/src/modules/ekf2/EKF/ekf_helper.cpp
@@ -1509,7 +1509,11 @@ void Ekf::stopGpsFusion()
 void Ekf::stopGpsPosFusion()
 {
 	_control_status.flags.gps = false;
-	_control_status.flags.gps_hgt = false;
+
+	if (_control_status.flags.gps_hgt) {
+		startBaroHgtFusion();
+	}
+
 	_gps_pos_innov.setZero();
 	_gps_pos_innov_var.setZero();
 	_gps_pos_test_ratio.setZero();