move aboveWater

() back to main

5 files changed, 14 insertions, 33 deletions

diff --git a/src/main.cpp b/src/main.cpp
index 4e61484..62d2560 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -27,6 +27,15 @@ using std::cout;
 using std::endl;
 using std::cerr;
 
+double 
+aboveWater(const Quaterniond &q)
+{
+    Vector3d tip;
+    tip << 0.65*3.43, 0, -0.60;
+    tip = q._transformVector(tip);
+    return tip[2];
+}
+
 #ifdef  USE_ROS
 
 #else      /* -----  not USE_ROS  ----- */
diff --git a/src/main.h b/src/main.h
index a78bab0..aeb4949 100644
--- a/src/main.h
+++ b/src/main.h
@@ -21,6 +21,7 @@ using Eigen::Matrix3d;
 using Eigen::Quaterniond;
 using Eigen::Vector3d;
 
+double aboveWater(const Quaterniond &q);
 int parseLine(char *line, message *msg);
 timestamp update_dt(const timestamp t, timestamp *t_old);
 
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;
diff --git a/src/types.cpp b/src/types.cpp
deleted file mode 100644
index 151fd63..0000000
--- a/src/types.cpp
+++ /dev/null
@@ -1,29 +0,0 @@
-/*
- * =====================================================================================
- *
- *       Filename:  types.cpp
- *
- *    Description:  
- *
- *        Version:  1.0
- *        Created:  03/27/2017 03:26:51 PM
- *       Revision:  none
- *       Compiler:  gcc
- *
- *         Author:  Martin Miller (MHM), miller7@illinois.edu
- *   Organization:  Aerospace Robotics and Controls Lab (ARC)
- *
- * =====================================================================================
- */
-
-
-#include "types.h"
-
-double 
-aboveWater(const Quaterniond &q)
-{
-    Vector3d tip;
-    tip << 0.65*3.43, 0, -0.60;
-    tip = q._transformVector(tip);
-    return tip[2];
-}
diff --git a/src/types.h b/src/types.h
index f32dfea..c284d4a 100644
--- a/src/types.h
+++ b/src/types.h
@@ -79,5 +79,4 @@ typedef struct {
     velocity_t velocity;
     covariance_t covariance;
 } message;
-double aboveWater(const Quaterniond &q);
 #endif   /* ----- #ifndef types_INC  ----- */