[opticflow] working variables in a struct

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

@@ -19,7 +19,7 @@
  */
 
 /**
- * @file modules/computer_vision/cv/framerate.h
+ * @file modules/computer_vision/cv/framerate.c
  *
  */
 
@@ -36,12 +36,6 @@ struct timeval end_time;
 
 #define USEC_PER_SEC 1000000L
 
-static float framerate_FPS;
-
-float framerate_get(void) {
-  return framerate_FPS;
-}
-
 static long time_elapsed(struct timeval *t1, struct timeval *t2)
 {
   long sec, usec;
@@ -68,15 +62,15 @@ static long end_timer(void)
 
 void framerate_init(void) {
   // Frame Rate Initialization
-  framerate_FPS = 0.0;
   timestamp = 0;
   start_timer();
 }
 
-void framerate_run(void) {
+float framerate_run(void) {
   // FPS
   timestamp = end_timer();
-  framerate_FPS = (float) 1000000 / (float)timestamp;
+  float framerate_FPS = (float) 1000000 / (float)timestamp;
   //  printf("dt = %d, FPS = %f\n",timestamp, FPS);
   start_timer();
+  return framerate_FPS;
 }

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

@@ -25,5 +25,4 @@
 
 
 void framerate_init(void);
-void framerate_run(void);
-float framerate_get(void);
+float framerate_run(void);

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

@@ -8,24 +8,26 @@
 
 // Data from thread to module
 struct CVresults {
-  int cnt;
-  int status;
-  float FPS;
-  float Velx;
+  int cnt;          // Number of processed frames
+
+  float Velx;       // Velocity as measured by camera
   float Vely;
   int flow_count;
-  float cam_h;
+
+  float cam_h;      // Debug parameters
   int count;
   float OFx, OFy, dx_sum, dy_sum;
-  float diff_roll, diff_pitch;
+  float diff_roll;
+  float diff_pitch;
+  float FPS;
 };
 
 // Data from module to thread
 struct PPRZinfo {
-  int cnt;
-  float theta;
-  float phi;
-  float agl;
+  int cnt;        // IMU msg counter
+  float phi;      // roll [rad]
+  float theta;    // pitch [rad]
+  float agl;      // height above ground [m]
 };
 
 

sw/airborne/modules/computer_vision/opticflow/opticflow_thread.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/opticflow/opticflow_thread.c
+ *
+ */
 
 
 
@@ -29,10 +53,8 @@
 #endif
 
 #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) {
@@ -43,23 +65,22 @@ void *computervision_thread_main(void *args)
 {
   int thread_socket = *(int *) args;
 
-  computer_vision_thread_command = RUN;
-
+  // Local data in/out
   struct CVresults vision_results;
   struct PPRZinfo autopilot_data;
 
+  // Status
+  computer_vision_thread_command = RUN;
+
   // Video Input
   struct vid_struct vid;
   vid.device = (char *)"/dev/video2"; // video1 = front camera; video2 = bottom camera
   vid.w = 320;
   vid.h = 240;
   vid.n_buffers = 4;
 
-  vision_results.status = 1;
-
   if (video_init(&vid) < 0) {
     printf("Error initialising video\n");
-    vision_results.status = -1;
     return 0;
   }
 
@@ -81,10 +102,11 @@ void *computervision_thread_main(void *args)
   opticflow_plugin_init(vid.w, vid.h, &vision_results);
 
   while (computer_vision_thread_command == RUN) {
-    vision_results.status = 2;
+
+    // Wait for a new frame
     video_grab_image(&vid, img_new);
 
-    // Get most recent State
+    // Get most recent State information
     int bytes_read = sizeof(autopilot_data);
     while (bytes_read == sizeof(autopilot_data))
     {
@@ -95,21 +117,19 @@ void *computervision_thread_main(void *args)
         }
       }
     }
-
     DEBUG_INFO("[thread] Read # %d\n",autopilot_data.cnt);
 
-    // Run Image Processing
+    // Run Image Processing with image and data and get results
     opticflow_plugin_run(img_new->buf, &autopilot_data, &vision_results);
 
-    /* send results to main */
+    /* Send results to main */
     vision_results.cnt++;
     int bytes_written = write(thread_socket, &vision_results, sizeof(vision_results));
     if (bytes_written != sizeof(vision_results)){
       perror("[thread] Failed to write to socket.\n");
     }
     DEBUG_INFO("[thread] Write # %d, (bytes %d)\n",vision_results.cnt, bytes_written);
 
-
 #ifdef DOWNLINK_VIDEO
     // JPEG encode the image:
     uint32_t quality_factor = 10; //20 if no resize,
@@ -119,12 +139,11 @@ void *computervision_thread_main(void *args)
     uint32_t size = end - (jpegbuf);
 
     printf("Sending an image ...%u\n", size);
-    uint32_t delta_t_per_frame = 0; // 0 = use drone clock
-    send_rtp_frame(vsock, jpegbuf, size, small.w, small.h, 0, quality_factor, dri_header, delta_t_per_frame);
+    send_rtp_frame(vsock, jpegbuf, size, small.w, small.h, 0, quality_factor, dri_header, 0);
 #endif
   }
+
   printf("Thread Closed\n");
   video_close(&vid);
-  vision_results.status = -100;
   return 0;
 }

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

@@ -19,11 +19,10 @@
  */
 
 /**
- * @file modules/computer_vision/opticflow/opticflow_thread.c
+ * @file modules/computer_vision/opticflow/opticflow_thread.h
  * @brief computer vision thread
  *
- * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
- */
+  */
 
 
 void *computervision_thread_main(void *args);  /* computer vision thread: should be given a pointer to a socketpair as argument */