From f51bf104823debea5a65403be949f9170f729b82 Mon Sep 17 00:00:00 2001
From: dave <david.st-onge@polymtl.ca>
Date: Sun, 18 Nov 2018 15:34:19 +0100
Subject: [PATCH] updated xbee list, takeoff height and hidden target gps list

---
 buzz_scripts/include/taskallocate/waypointlist.csv | 14 ++++++++++----
 buzz_scripts/include/utils/takeoff_heights.bzz     | 11 ++++++++++-
 src/buzzuav_closures.cpp                           | 13 ++++++++++---
 3 files changed, 30 insertions(+), 8 deletions(-)

diff --git a/buzz_scripts/include/taskallocate/waypointlist.csv b/buzz_scripts/include/taskallocate/waypointlist.csv
index a11120b..3d198b7 100644
--- a/buzz_scripts/include/taskallocate/waypointlist.csv
+++ b/buzz_scripts/include/taskallocate/waypointlist.csv
@@ -1,4 +1,10 @@
-1,-73.609219,45.510336,1
-2,-73.608913,45.510723,0
-3,-73.610035,45.510182,1
-4,-73.609235,45.510114,0
\ No newline at end of file
+1,-13.710661,29.020850,10.0,1
+2,-13.711061,29.020946,10.0,0
+3,-13.711605,29.020421,10.0,1
+4,-13.711925,29.020003,10.0,0
+5,-13.712166,29.020435,10.0,1
+6,-13.710498,29.020341,10.0,0
+7,-13.711506,29.019919,10.0,0
+8,-13.712048,29.020385,10.0,0
+9,-13.711514,29.020261,10.0,1
+10,-13.711621,29.020725,10.0,1
\ No newline at end of file
diff --git a/buzz_scripts/include/utils/takeoff_heights.bzz b/buzz_scripts/include/utils/takeoff_heights.bzz
index 5e760d8..03aaaa0 100644
--- a/buzz_scripts/include/utils/takeoff_heights.bzz
+++ b/buzz_scripts/include/utils/takeoff_heights.bzz
@@ -12,5 +12,14 @@ takeoff_heights ={
     .16 = 9.0,
     .17 = 3.0,
     .18 = 6.0,
-    .19 = 9.0
+    .19 = 9.0,
+    .20 = 3.0,
+    .21 = 3.0,
+    .22 = 3.0,
+    .23 = 0.0,
+    .24 = 38.0,
+    .25 = 34.0,
+    .26 = 25.0,
+    .27 = 42.0,
+    .28 = 29.0
 }
\ No newline at end of file
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index 144af11..baf6f4f 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -195,12 +195,13 @@ void parse_gpslist()
     double lon = atof(strtok(NULL, DELIMS));
     double lat = atof(strtok(NULL, DELIMS));
     int alt = atoi(strtok(NULL, DELIMS));
-    // int tilt = atoi(strtok(NULL, DELIMS));
+    int tilt = atoi(strtok(NULL, DELIMS));
     //  DEBUG
     // ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid);
     RB_arr.latitude = lat;
     RB_arr.longitude = lon;
     RB_arr.altitude = alt;
+    RB_arr.r = tilt;
     // Insert elements.
     map<int, buzz_utility::RB_struct>::iterator it = wplist_map.find(tid);
     if (it != wplist_map.end())
@@ -227,8 +228,14 @@ void check_targets_sim(double lat, double lon, double *res)
     double ref[2]={lat, lon};
     double tar[2]={it->second.latitude, it->second.longitude};
     rb_from_gps(tar, rb, ref);
-    if(rb[0] < visibility_radius){
-      ROS_WARN("FOUND A TARGET!!! [%i]", it->first);
+    if(rb[0] < visibility_radius && (buzz_utility::get_bvmstate()=="WAYPOINT" && it->second.r==0)){
+      ROS_WARN("FOUND A TARGET IN WAYPOINT!!! [%i]", it->first);
+      res[0] = it->first;
+      res[1] = it->second.latitude;
+      res[2] = it->second.longitude;
+      res[3] = it->second.altitude;
+    } else if(rb[0] < visibility_radius && (buzz_utility::get_bvmstate()=="DEPLOY" && it->second.r==1)){
+      ROS_WARN("FOUND A TARGET IN WAYPOINT!!! [%i]", it->first);
       res[0] = it->first;
       res[1] = it->second.latitude;
       res[2] = it->second.longitude;