From 9c4dd024bf274800e4ef9e4487d58d2d372530d7 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 5 May 2016 19:46:33 +1000
Subject: [PATCH] AP_NavEKF2: auto change EK2_GPS_TYPE for NMEA

this fixes a problem where users of NMEA GPS receivers could not arm
with default EK2 parameters.
---
 libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
index eac51492c8..f22e9a892c 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
@@ -8,6 +8,7 @@
 #include "AP_NavEKF2_core.h"
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Vehicle/AP_Vehicle.h>
+#include <GCS_MAVLink/GCS.h>
 
 #include <stdio.h>
 
@@ -78,6 +79,13 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
     } else if ((frontend->_fusionModeGPS == 0) && !_ahrs->get_gps().have_vertical_velocity()) {
         // If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail
         gpsVertVelFail = true;
+        // if we have a 3D fix with no vertical velocity and
+        // EK2_GPS_TYPE=0 then change it to 1. It means the GPS is not
+        // capable of giving a vertical velocity
+        if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
+            frontend->_fusionModeGPS.set(1);
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EK2: Changed EK2_GPS_TYPE to 1");
+        }
     } else {
         gpsVertVelFail = false;
     }