From 7f15b7b126f1e2871fd99733fb53f3b023038a91 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 27 Nov 2020 17:21:34 +0900
Subject: [PATCH] AP_NavEKF_Source: prearm check of wheelencoders

---
 libraries/AP_NavEKF/AP_NavEKF_Source.cpp | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
index 1d5294b163..5895a54d01 100644
--- a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
@@ -323,6 +323,7 @@ bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
     bool rangefinder_required = false;
     bool visualodom_required = false;
     bool optflow_required = false;
+    bool wheelencoder_required = false;
 
     // check source params are valid
     for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
@@ -362,7 +363,7 @@ bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
             visualodom_required = true;
             break;
         case SourceXY::WHEEL_ENCODER:
-            // ToDo: add wheelencoder_required and test below
+            wheelencoder_required = true;
             break;
         case SourceXY::BEACON:
         default:
@@ -475,6 +476,11 @@ bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
         }
     }
 
+    if (wheelencoder_required && !dal.wheelencoder_enabled()) {
+        hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "WheelEncoder");
+        return false;
+    }
+
     return true;
 }