Messages in module instead of thread

sw/airborne/modules/computer_vision/cv/framerate.c

@@ -1,3 +1,27 @@
+/*
+ * Copyright (C) 2015 The Paparazzi Community
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file modules/computer_vision/cv/framerate.h
+ *
+ */
 
 #include "std.h"
 #include "framerate.h"
@@ -12,7 +36,11 @@ struct timeval end_time;
 
 #define USEC_PER_SEC 1000000L
 
-float FPS;
+static float framerate_FPS;
+
+float framerate_get(void) {
+  return framerate_FPS;
+}
 
 static long time_elapsed(struct timeval *t1, struct timeval *t2)
 {
@@ -40,7 +68,7 @@ static long end_timer(void)
 
 void framerate_init(void) {
   // Frame Rate Initialization
-  FPS = 0.0;
+  framerate_FPS = 0.0;
   timestamp = 0;
   start_timer();
 }
@@ -48,7 +76,7 @@ void framerate_init(void) {
 void framerate_run(void) {
   // FPS
   timestamp = end_timer();
-  FPS = (float) 1000000 / (float)timestamp;
+  framerate_FPS = (float) 1000000 / (float)timestamp;
   //  printf("dt = %d, FPS = %f\n",timestamp, FPS);
   start_timer();
 }

sw/airborne/modules/computer_vision/cv/framerate.h

@@ -1,5 +1,29 @@
+/*
+ * Copyright (C) 2015 The Paparazzi Community
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file modules/computer_vision/cv/framerate.h
+ *
+ */
 
-extern float FPS;
 
 void framerate_init(void);
 void framerate_run(void);
+float framerate_get(void);

sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c

@@ -142,10 +142,6 @@ void init_hover_stabilization_onvision()
 
 void run_hover_stabilization_onvision(struct CVresults* vision )
 {
-  if (autopilot_mode != AP_MODE_MODULE) {
-    return;
-  }
-
   struct FloatVect3 V_body;
   if (activate_opticflow_hover == TRUE) {
     // Compute body velocities from ENU
@@ -154,6 +150,21 @@ void run_hover_stabilization_onvision(struct CVresults* vision )
     float_quat_vmult(&V_body, q_n2b, vel_ned);
   }
 
+  // *************************************************************************************
+  // Downlink Message
+  // *************************************************************************************
+
+  DOWNLINK_SEND_OF_HOVER(DefaultChannel, DefaultDevice,
+                         &vision->FPS, &vision->dx_sum, &vision->dy_sum, &vision->OFx, &vision->OFy,
+                         &vision->diff_roll, &vision->diff_pitch,
+                         &vision->Velx, &vision->Vely,
+                         &V_body.x, &V_body.y,
+                         &vision->cam_h, &vision->count);
+
+  if (autopilot_mode != AP_MODE_MODULE) {
+    return;
+  }
+
   if (vision->flow_count) {
     Error_Velx = vision->Velx - vision_desired_vx;
     Error_Vely = vision->Vely - vision_desired_vy;

sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h

@@ -10,9 +10,14 @@
 struct CVresults {
   int cnt;
   int status;
+  float FPS;
   float Velx;
   float Vely;
   int flow_count;
+  float cam_h;
+  int count;
+  float OFx, OFy, dx_sum, dy_sum;
+  float diff_roll, diff_pitch;
 };
 
 // Data from module to thread

sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c

@@ -30,10 +30,13 @@
 
 #include <stdio.h>
 
+#define DEBUG_INFO(X, ...) ;
+
+
 static volatile enum{RUN,EXIT} computer_vision_thread_command = RUN;  /** request to close: set to 1 */
 
 void computervision_thread_request_exit(void) {
-  computer_vision_thread_command = 0;
+  computer_vision_thread_command = EXIT;
 }
 
 void *computervision_thread_main(void *args)
@@ -93,7 +96,7 @@ void *computervision_thread_main(void *args)
       }
     }
 
-    printf("[thread] Read # %d\n",autopilot_data.cnt);
+    DEBUG_INFO("[thread] Read # %d\n",autopilot_data.cnt);
 
     // Run Image Processing
     opticflow_plugin_run(img_new->buf, &autopilot_data, &vision_results);
@@ -104,7 +107,7 @@ void *computervision_thread_main(void *args)
     if (bytes_written != sizeof(vision_results)){
       perror("[thread] Failed to write to socket.\n");
     }
-    printf("[thread] Write # %d, (bytes %d)\n",vision_results.cnt, bytes_written);
+    DEBUG_INFO("[thread] Write # %d, (bytes %d)\n",vision_results.cnt, bytes_written);
 
 
 #ifdef DOWNLINK_VIDEO

sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h

@@ -26,7 +26,5 @@
  */
 
 
-#include "std.h"
-
 void *computervision_thread_main(void *args);  /* computer vision thread: should be given a pointer to a socketpair as argument */
 void computervision_thread_request_exit(void);

sw/airborne/modules/computer_vision/opticflow/visual_estimator.c

@@ -47,7 +47,6 @@ unsigned int imgWidth, imgHeight;
 // Local variables
 unsigned char *prev_frame, *gray_frame, *prev_gray_frame;
 int old_img_init;
-float OFx, OFy, dx_sum, dy_sum;
 
 // ARDrone Vertical Camera Parameters
 #define FOV_H 0.67020643276
@@ -57,7 +56,6 @@ float OFx, OFy, dx_sum, dy_sum;
 
 // Corner Detection
 int *x, *y;
-int count = 0;
 int max_count = 25;
 #define MAX_COUNT 100
 
@@ -74,7 +72,7 @@ float distance2, min_distance, min_distance2;
 // Flow Derotation
 #define FLOW_DEROTATION
 float curr_pitch, curr_roll, prev_pitch, prev_roll;
-float cam_h, diff_roll, diff_pitch, OFx_trans, OFy_trans;
+float OFx_trans, OFy_trans;
 
 
 
@@ -95,13 +93,13 @@ void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *res
   dx = (int *) calloc(MAX_COUNT, sizeof(int));
   dy = (int *) calloc(MAX_COUNT, sizeof(int));
   old_img_init = 1;
-  OFx = 0.0;
-  OFy = 0.0;
-  dx_sum = 0.0;
-  dy_sum = 0.0;
-  diff_roll = 0.0;
-  diff_pitch = 0.0;
-  cam_h = 0.0;
+  results->OFx = 0.0;
+  results->OFy = 0.0;
+  results->dx_sum = 0.0;
+  results->dy_sum = 0.0;
+  results->diff_roll = 0.0;
+  results->diff_pitch = 0.0;
+  results->cam_h = 0.0;
   prev_pitch = 0.0;
   prev_roll = 0.0;
   curr_pitch = 0.0;
@@ -113,6 +111,7 @@ void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *res
   results->flow_count = 0;
   results->cnt = 0;
   results->status = 0;
+  results->count = 0;
 
   framerate_init();
 }
@@ -135,10 +134,10 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV
   int fast_threshold = 20;
   xyFAST *pnts_fast;
   pnts_fast = fast9_detect((const byte *)prev_gray_frame, imgWidth, imgHeight, imgWidth,
-                           fast_threshold, &count);
+                           fast_threshold, &results->count);
 
-  if (count > MAX_COUNT) { count = MAX_COUNT; }
-  for (int i = 0; i < count; i++) {
+  if (results->count > MAX_COUNT) { results->count = MAX_COUNT; }
+  for (int i = 0; i < results->count; i++) {
     x[i] = pnts_fast[i].x;
     y[i] = pnts_fast[i].y;
   }
@@ -149,8 +148,8 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV
   min_distance2 = min_distance * min_distance;
   int *labelmin;
   labelmin = (int *) calloc(MAX_COUNT, sizeof(int));
-  for (int i = 0; i < count; i++) {
-    for (int j = i + 1; j < count; j++) {
+  for (int i = 0; i < results->count; i++) {
+    for (int j = i + 1; j < results->count; j++) {
       // distance squared:
       distance2 = (x[i] - x[j]) * (x[i] - x[j]) + (y[i] - y[j]) * (y[i] - y[j]);
       if (distance2 < min_distance2) {
@@ -159,8 +158,8 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV
     }
   }
 
-  int count_fil = count;
-  for (int i = count - 1; i >= 0; i--) {
+  int count_fil = results->count;
+  for (int i = results->count - 1; i >= 0; i--) {
     remove_point = 0;
 
     if (labelmin[i]) {
@@ -177,7 +176,7 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV
   }
 
   if (count_fil > max_count) { count_fil = max_count; }
-  count = count_fil;
+  results->count = count_fil;
   free(labelmin);
 
   // *************************************************************************************
@@ -208,8 +207,8 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV
     }
   }
 
-  dx_sum = 0.0;
-  dy_sum = 0.0;
+  results->dx_sum = 0.0;
+  results->dy_sum = 0.0;
 
   // Optical Flow Computation
   for (int i = 0; i < results->flow_count; i++) {
@@ -222,35 +221,35 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV
     quick_sort_int(dx, results->flow_count); // 11
     quick_sort_int(dy, results->flow_count); // 11
 
-    dx_sum = (float) dx[results->flow_count / 2];
-    dy_sum = (float) dy[results->flow_count / 2];
+    results->dx_sum = (float) dx[results->flow_count / 2];
+    results->dy_sum = (float) dy[results->flow_count / 2];
   } else {
-    dx_sum = 0.0;
-    dy_sum = 0.0;
+    results->dx_sum = 0.0;
+    results->dy_sum = 0.0;
   }
 
   // Flow Derotation
   curr_pitch = info->theta;
   curr_roll = info->phi;
 
-  diff_pitch = (curr_pitch - prev_pitch) * imgHeight / FOV_H;
-  diff_roll = (curr_roll - prev_roll) * imgWidth / FOV_W;
+  results->diff_pitch = (curr_pitch - prev_pitch) * imgHeight / FOV_H;
+  results->diff_roll = (curr_roll - prev_roll) * imgWidth / FOV_W;
 
   prev_pitch = curr_pitch;
   prev_roll = curr_roll;
 
 #ifdef FLOW_DEROTATION
   if (results->flow_count) {
-    OFx_trans = dx_sum - diff_roll;
-    OFy_trans = dy_sum - diff_pitch;
+    OFx_trans = results->dx_sum - results->diff_roll;
+    OFy_trans = results->dy_sum - results->diff_pitch;
 
-    if ((OFx_trans <= 0) != (dx_sum <= 0)) {
+    if ((OFx_trans <= 0) != (results->dx_sum <= 0)) {
       OFx_trans = 0;
       OFy_trans = 0;
     }
   } else {
-    OFx_trans = dx_sum;
-    OFy_trans = dy_sum;
+    OFx_trans = results->dx_sum;
+    OFy_trans = results->dy_sum;
   }
 #else
   OFx_trans = dx_sum;
@@ -258,19 +257,21 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV
 #endif
 
   // Average Filter
-  OFfilter(&OFx, &OFy, OFx_trans, OFy_trans, results->flow_count, 1);
+  OFfilter(&results->OFx, &results->OFy, OFx_trans, OFy_trans, results->flow_count, 1);
 
   // Velocity Computation
   if (info->agl < 0) {
-    cam_h = 1;
+    results->cam_h = 1;
   }
   else {
-    cam_h = info->agl;
+    results->cam_h = info->agl;
   }
 
+  results->FPS = framerate_get();
+
   if (results->flow_count) {
-    results->Velx = OFy * cam_h * FPS / Fy_ARdrone + 0.05;
-    results->Vely = -OFx * cam_h * FPS / Fx_ARdrone - 0.1;
+    results->Velx = results->OFy * results->cam_h * results->FPS / Fy_ARdrone + 0.05;
+    results->Vely = -results->OFx * results->cam_h * results->FPS / Fx_ARdrone - 0.1;
   } else {
     results->Velx = 0.0;
     results->Vely = 0.0;
@@ -283,13 +284,5 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV
   memcpy(prev_frame, frame, imgHeight * imgWidth * 2);
   memcpy(prev_gray_frame, gray_frame, imgHeight * imgWidth);
 
-#if 0
-  // *************************************************************************************
-  // Downlink Message
-  // *************************************************************************************
-  DOWNLINK_SEND_OF_HOVER(DefaultChannel, DefaultDevice, &FPS, &dx_sum, &dy_sum, &OFx, &OFy,
-                         &diff_roll, &diff_pitch, &Velx, &Vely, &V_body.x, &V_body.y,
-                         &cam_h, &count);
-#endif
 }
 

sw/airborne/modules/computer_vision/opticflow_module.c

@@ -74,6 +74,7 @@ static void agl_cb(uint8_t sender_id __attribute__((unused)), const float *dista
   }
 }
 
+#define DEBUG_INFO(X, ...) ;
 
 void opticflow_module_init(void)
 {
@@ -102,7 +103,7 @@ void opticflow_module_run(void)
     printf("[module] Failed to write to socket: written = %d, error=%d.\n",bytes_written, errno);
   }
   else {
-    printf("[module] Write # %d (%d bytes)\n",opticflow_module_data.cnt, bytes_written);
+    DEBUG_INFO("[module] Write # %d (%d bytes)\n",opticflow_module_data.cnt, bytes_written);
   }
 
   // Read Latest Vision Module Results
@@ -117,7 +118,7 @@ void opticflow_module_run(void)
     ////////////////////////////////////////////
     // Module-Side Code
     ////////////////////////////////////////////
-    printf("[module] Read vision %d\n",vision_results.cnt);
+    DEBUG_INFO("[module] Read vision %d\n",vision_results.cnt);
     run_hover_stabilization_onvision(&vision_results);
   }
 }

sw/airborne/modules/computer_vision/opticflow_module.h

@@ -36,7 +36,4 @@ extern void opticflow_module_run(void);
 extern void opticflow_module_start(void);
 extern void opticflow_module_stop(void);
 
-/// Frame Rate
-extern float FPS;
-
 #endif /* OPTICFLOW_MODULE_H */