From fb4b8328bfa5a8930b87ec8b7465b8032f4873c5 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Tue, 28 Mar 2017 10:06:47 -0500
Subject: move aboveWater () back to main

---
 src/state.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

(limited to 'src/state.cpp')

diff --git a/src/state.cpp b/src/state.cpp
index aee0caa..aaf9cb9 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -260,9 +260,11 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
 {
     std::vector<measurement_t> featuresToAdd;
     std::vector<measurement_t> featuresToUpdate;
+    double zmeas = body->ned()[2];
     for (auto i=z.begin(); i!=z.end(); ++i) {
         if (i->z_type==HEIGHT) {
             featuresToUpdate.push_back(*i);
+            zmeas = i->height;
         } else if (exists(i->id)) {
             featuresToUpdate.push_back(*i);
         } else {
@@ -280,16 +282,15 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
     
 #endif     /* -----  not FULLS  ----- */
     }
-    addFeatures( featuresToAdd, q);
+    addFeatures( featuresToAdd, q, zmeas);
 
     return ;
 }		/* -----  end of method State::feature_update  ----- */
 
 void
-State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q)
+State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q, double z)
 {
     // Add new features
-    double z = aboveWater(q);
     Vector3d pos = body->ned();
     for (auto i=F.begin(); i!=F.end(); ++i) {
         if (features.size()>MAXFEATURES) break;
-- 
cgit v1.1