From 2542259a445d5084779dac674568e5b4c8374178 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Mon, 14 Sep 2015 13:57:35 +0200
Subject: [PATCH] [blob] switch algorithm

---
 conf/modules/cv_blob_locator.xml              | 17 ++++++++++------
 .../modules/computer_vision/blob/imavmarker.c |  9 +++++++++
 .../modules/computer_vision/blob/imavmarker.h |  2 ++
 .../modules/computer_vision/cv_blob_locator.c | 20 +++++++++++++++++++
 .../modules/computer_vision/cv_blob_locator.h |  1 +
 5 files changed, 43 insertions(+), 6 deletions(-)

diff --git a/conf/modules/cv_blob_locator.xml b/conf/modules/cv_blob_locator.xml
index bc3f26fb8b4..0e0b860cdac 100644
--- a/conf/modules/cv_blob_locator.xml
+++ b/conf/modules/cv_blob_locator.xml
@@ -7,15 +7,19 @@
   <settings>
     <dl_settings>
       <dl_settings NAME="ColorFilter">
-         <dl_setting var="color_lum_min" min="0" step="1" max="255" shortname="y_min" />
-         <dl_setting var="color_lum_max" min="0" step="1" max="255" shortname="y_max" />
-         <dl_setting var="color_cb_min"  min="0" step="1" max="255" shortname="u_min" />
-         <dl_setting var="color_cb_max"  min="0" step="1" max="255" shortname="u_max" />
-         <dl_setting var="color_cr_min"  min="0" step="1" max="255" shortname="v_min" />
-         <dl_setting var="color_cr_max"  min="0" step="1" max="255" shortname="v_max" />
+        <dl_setting var="color_lum_min" min="0" step="1" max="255" shortname="y_min" />
+        <dl_setting var="color_lum_max" min="0" step="1" max="255" shortname="y_max" />
+        <dl_setting var="color_cb_min"  min="0" step="1" max="255" shortname="u_min" />
+        <dl_setting var="color_cb_max"  min="0" step="1" max="255" shortname="u_max" />
+        <dl_setting var="color_cr_min"  min="0" step="1" max="255" shortname="v_min" />
+        <dl_setting var="color_cr_max"  min="0" step="1" max="255" shortname="v_max" />
         <dl_setting var="cv_blob_locator_reset" max="1" min="0" step="1" module="computer_vision/cv_blob_locator"  handler="GeoReset">
           <strip_button name="Reset" value="1" />
         </dl_setting>
+        <dl_setting var="cv_blob_locator_type" min="0" step="1" max="1" module="computer_vision/cv_blob_locator" values="BLOB|IMAVMARKER">
+          <strip_button name="POWER ON" icon="on.png" value="1" group="power_switch"/>
+          <strip_button name="POWER OFF" icon="off.png" value="0" group="power_switch"/>
+        </dl_setting>
       </dl_settings>
     </dl_settings>
   </settings>
@@ -30,6 +34,7 @@
   <event fun="cv_blob_locator_event()"/>
   <makefile>
     <file name="cv_blob_locator.c"/>
+    <file name="imavmarker.c" dir="modules/computer_vision/blob" />
     <file name="blob_finder.c" dir="modules/computer_vision/blob" />
     <file name="cv_georeference.c" dir="modules/computer_vision/" />
   </makefile>
diff --git a/sw/airborne/modules/computer_vision/blob/imavmarker.c b/sw/airborne/modules/computer_vision/blob/imavmarker.c
index 327ed53efef..ac244ae4f53 100644
--- a/sw/airborne/modules/computer_vision/blob/imavmarker.c
+++ b/sw/airborne/modules/computer_vision/blob/imavmarker.c
@@ -35,6 +35,8 @@
  * @return The deviation of the marker location wrt the center.
  */
 
+bool_t marker_enabled = TRUE;
+
 
 #define Img(X,Y)(source[(Y)*input->w*2+(X)*2+1])
 #define Out(X,Y)(source[(Y)*input->w*2+(X)*2])
@@ -52,6 +54,13 @@ struct marker_deviation_t marker(struct image_t *input, uint8_t M)
 {
   struct marker_deviation_t marker_deviation;
 
+  marker_deviation.x = 0;
+  marker_deviation.y = 0;
+  marker_deviation.inlier = 0;
+
+  if (! marker_enabled)
+    return marker_deviation;
+
   uint8_t *source = (uint8_t *) input->buf;
   uint16_t x, y, i, j, k;
 
diff --git a/sw/airborne/modules/computer_vision/blob/imavmarker.h b/sw/airborne/modules/computer_vision/blob/imavmarker.h
index bb5d25a7955..d0d47c9ea8e 100644
--- a/sw/airborne/modules/computer_vision/blob/imavmarker.h
+++ b/sw/airborne/modules/computer_vision/blob/imavmarker.h
@@ -35,4 +35,6 @@ struct marker_deviation_t {
 };
 
 
+extern bool_t marker_enabled;
+
 struct marker_deviation_t marker(struct image_t *input, uint8_t M);
diff --git a/sw/airborne/modules/computer_vision/cv_blob_locator.c b/sw/airborne/modules/computer_vision/cv_blob_locator.c
index 51c34ad60c5..d870c1d1bd2 100644
--- a/sw/airborne/modules/computer_vision/cv_blob_locator.c
+++ b/sw/airborne/modules/computer_vision/cv_blob_locator.c
@@ -26,6 +26,7 @@
 #include "modules/computer_vision/cv_blob_locator.h"
 #include "modules/computer_vision/cv.h"
 #include "modules/computer_vision/blob/blob_finder.h"
+#include "modules/computer_vision/blob/imavmarker.h"
 
 
 uint8_t color_lum_min;
@@ -38,13 +39,22 @@ uint8_t color_cr_min;
 uint8_t color_cr_max;
 
 uint8_t cv_blob_locator_reset;
+uint8_t cv_blob_locator_type;
 
 volatile uint32_t blob_locator = 0;
 
+volatile bool_t blob_enabled = FALSE;
+
 // Computer vision thread
 bool_t cv_blob_locator_func(struct image_t *img);
 bool_t cv_blob_locator_func(struct image_t *img) {
 
+  blob_enabled = ! marker_enabled;
+
+  if (!blob_enabled)
+    return FALSE;
+
+
   // Color Filter
   struct image_filter_t filter[2];
   filter[0].y_min = color_lum_min;
@@ -168,6 +178,16 @@ void cv_blob_locator_periodic(void) {
 
 
 void cv_blob_locator_event(void) {
+  if (cv_blob_locator_type == 1)
+  {
+    blob_enabled = TRUE;
+    marker_enabled = FALSE;
+  }
+  else
+  {
+    blob_enabled = FALSE;
+    marker_enabled = TRUE;
+  }
   if (blob_locator != 0) {
     // CV thread has results: import
     uint32_t temp = blob_locator;
diff --git a/sw/airborne/modules/computer_vision/cv_blob_locator.h b/sw/airborne/modules/computer_vision/cv_blob_locator.h
index f6c788100d3..5276111028c 100644
--- a/sw/airborne/modules/computer_vision/cv_blob_locator.h
+++ b/sw/airborne/modules/computer_vision/cv_blob_locator.h
@@ -38,6 +38,7 @@ extern uint8_t color_cr_min;
 extern uint8_t color_cr_max;
 
 extern uint8_t cv_blob_locator_reset;
+extern uint8_t cv_blob_locator_type;
 
 extern void cv_blob_locator_init(void);
 extern void cv_blob_locator_periodic(void);