convert to thread-communication

conf/modules/cv_opticflow.xml

@@ -43,16 +43,18 @@
 
   <init fun="opticflow_module_init()"/>
 
-  <periodic fun="opticflow_module_run()" freq="60" start="opticflow_module_start()" stop="opticflow_module_stop()" autorun="TRUE"/>
+  <periodic fun="opticflow_module_run()" start="opticflow_module_start()" stop="opticflow_module_stop()" autorun="TRUE"/>
   <makefile target="ap">
     <define name="ARDRONE_VIDEO_PORT" value="2002" />
 
     <file name="opticflow_module.c"/>
+    <file name="opticflow_thread.c" dir="modules/computer_vision/opticflow"/>
     <file name="visual_estimator.c" dir="modules/computer_vision/opticflow"/>
     <file name="hover_stabilization.c" dir="modules/computer_vision/opticflow"/>
     <file name="optic_flow_int.c" dir="modules/computer_vision/cv/opticflow"/>
     <file name="fastRosten.c" dir="modules/computer_vision/cv/opticflow/fast9"/>
     <file name="trig.c" dir="modules/computer_vision/cv"/>
+    <file name="framerate.c" dir="modules/computer_vision/cv"/>
     <file name="jpeg.c" dir="modules/computer_vision/cv/encoding"/>
     <file name="rtp.c" dir="modules/computer_vision/cv/encoding"/>
     <file name="socket.c" dir="modules/computer_vision/lib/udp"/>

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

@@ -0,0 +1,54 @@
+
+#include "std.h"
+#include "framerate.h"
+
+// Frame Rate (FPS)
+#include <sys/time.h>
+
+// local variables
+volatile long timestamp;
+struct timeval start_time;
+struct timeval end_time;
+
+#define USEC_PER_SEC 1000000L
+
+float FPS;
+
+static long time_elapsed(struct timeval *t1, struct timeval *t2)
+{
+  long sec, usec;
+  sec = t2->tv_sec - t1->tv_sec;
+  usec = t2->tv_usec - t1->tv_usec;
+  if (usec < 0) {
+    --sec;
+    usec = usec + USEC_PER_SEC;
+  }
+  return sec * USEC_PER_SEC + usec;
+}
+
+static void start_timer(void)
+{
+  gettimeofday(&start_time, NULL);
+}
+
+static long end_timer(void)
+{
+  gettimeofday(&end_time, NULL);
+  return time_elapsed(&start_time, &end_time);
+}
+
+
+void framerate_init(void) {
+  // Frame Rate Initialization
+  FPS = 0.0;
+  timestamp = 0;
+  start_timer();
+}
+
+void framerate_run(void) {
+  // FPS
+  timestamp = end_timer();
+  FPS = (float) 1000000 / (float)timestamp;
+  //  printf("dt = %d, FPS = %f\n",timestamp, FPS);
+  start_timer();
+}

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

@@ -0,0 +1,5 @@
+
+extern float FPS;
+
+void framerate_init(void);
+void framerate_run(void);

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

@@ -29,11 +29,7 @@
 // Own Header
 #include "hover_stabilization.h"
 
-// Vision Data
-#include "visual_estimator.h"
-
 // Stabilization
-//#include "stabilization.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 #include "firmwares/rotorcraft/guidance/guidance_v.h"
 #include "autopilot.h"
@@ -144,15 +140,12 @@ void init_hover_stabilization_onvision()
   Vely_Int = 0;
 }
 
-void run_hover_stabilization_onvision(void)
+void run_hover_stabilization_onvision(struct CVresults* vision )
 {
-  if (autopilot_mode == AP_MODE_MODULE) {
-    run_opticflow_hover();
+  if (autopilot_mode != AP_MODE_MODULE) {
+    return;
   }
-}
 
-void run_opticflow_hover(void)
-{
   struct FloatVect3 V_body;
   if (activate_opticflow_hover == TRUE) {
     // Compute body velocities from ENU
@@ -161,9 +154,9 @@ void run_opticflow_hover(void)
     float_quat_vmult(&V_body, q_n2b, vel_ned);
   }
 
-  if (flow_count) {
-    Error_Velx = Velx - vision_desired_vx;
-    Error_Vely = Vely - vision_desired_vy;
+  if (vision->flow_count) {
+    Error_Velx = vision->Velx - vision_desired_vx;
+    Error_Vely = vision->Vely - vision_desired_vy;
   } else {
     Error_Velx = 0;
     Error_Vely = 0;
@@ -204,6 +197,6 @@ void run_opticflow_hover(void)
   else if (cmd_euler.theta > CMD_OF_SAT) {cmd_euler.theta = CMD_OF_SAT; saturateY = 1;}
 
   stabilization_attitude_set_rpy_setpoint_i(&cmd_euler);
-  DOWNLINK_SEND_VISION_STABILIZATION(DefaultChannel, DefaultDevice, &Velx, &Vely, &Velx_Int,
+  DOWNLINK_SEND_VISION_STABILIZATION(DefaultChannel, DefaultDevice, &vision->Velx, &vision->Vely, &Velx_Int,
                                      &Vely_Int, &cmd_euler.phi, &cmd_euler.theta);
 }

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

@@ -30,6 +30,7 @@
 #define HOVER_STABILIZATION_H_
 
 #include <std.h>
+#include "inter_thread_data.h"
 
 // Controller module
 
@@ -46,7 +47,7 @@ extern void guidance_h_module_run(bool_t in_flight);
 
 
 void init_hover_stabilization_onvision(void);
-void run_hover_stabilization_onvision(void);
+void run_hover_stabilization_onvision(struct CVresults *vision);
 
 extern bool activate_opticflow_hover;
 extern float vision_desired_vx;

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

@@ -0,0 +1,28 @@
+
+
+#ifndef _INTER_THREAD_DATA_H
+#define _INTER_THREAD_DATA_H
+
+
+// Inter-thread communication: Unix Socket
+
+// Data from thread to module
+struct CVresults {
+  int x;
+  int status;
+  float Velx;
+  float Vely;
+  int flow_count;
+};
+
+// Data from module to thread
+struct PPRZinfo {
+  int cnt;
+  float theta;
+  float phi;
+  float agl;
+};
+
+
+
+#endif

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

@@ -0,0 +1,121 @@
+
+
+
+// Sockets
+#include <stdio.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+
+#include "opticflow_thread.h"
+
+
+/////////////////////////////////////////////////////////////////////////
+// COMPUTER VISION THREAD
+
+// Video
+#include "v4l/video.h"
+#include "resize.h"
+
+// Payload Code
+#include "visual_estimator.h"
+
+// Downlink Video
+//#define DOWNLINK_VIDEO 1
+
+#ifdef DOWNLINK_VIDEO
+#include "encoding/jpeg.h"
+#include "encoding/rtp.h"
+#endif
+
+#include <stdio.h>
+
+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;
+}
+
+void *computervision_thread_main(void *args)
+{
+  int thread_socket = *(int *) args;
+  int cnt = 0;
+
+  computer_vision_thread_command = RUN;
+
+  struct CVresults vision_results;
+  struct PPRZinfo autopilot_data;
+
+  // 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;
+  }
+
+  // Video Grabbing
+  struct img_struct *img_new = video_create_image(&vid);
+
+#ifdef DOWNLINK_VIDEO
+  // Video Compression
+  uint8_t *jpegbuf = (uint8_t *)malloc(vid.h * vid.w * 2);
+
+  // Network Transmit
+  struct UdpSocket *vsock;
+  //#define FMS_UNICAST 0
+  //#define FMS_BROADCAST 1
+  vsock = udp_socket("192.168.1.255", 5000, 5001, FMS_BROADCAST);
+#endif
+
+  // First Apply Settings before init
+  opticflow_plugin_init(vid.w, vid.h, &vision_results);
+
+  while (computer_vision_thread_command > 0) {
+    vision_results.status = 2;
+    video_grab_image(&vid, img_new);
+
+    // Get most recent State
+    int bytes_read = read(thread_socket, &autopilot_data, sizeof(autopilot_data));
+    if (bytes_read <= sizeof(autopilot_data)) {
+      if (bytes_read != 0) {
+        printf("Failed to read PPRZ info from socket.\n");
+      }
+    }
+
+    // Run Image Processing
+    opticflow_plugin_run(img_new->buf, &autopilot_data, &vision_results);
+
+    /* send results to main */
+    vision_results.x = cnt++;
+    int bytes_written = write(thread_socket, &vision_results, sizeof(vision_results));
+    if (bytes_written != sizeof(vision_results)){
+      perror("failed to write to socket.\n");
+    }
+
+
+#ifdef DOWNLINK_VIDEO
+    // JPEG encode the image:
+    uint32_t quality_factor = 10; //20 if no resize,
+    uint8_t dri_header = 0;
+    uint32_t image_format = FOUR_TWO_TWO;  // format (in jpeg.h)
+    uint8_t *end = encode_image(small.buf, jpegbuf, quality_factor, image_format, small.w, small.h, dri_header);
+    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);
+#endif
+  }
+  printf("Thread Closed\n");
+  video_close(&vid);
+  vision_results.status = -100;
+  return 0;
+}

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

@@ -0,0 +1,32 @@
+/*
+ * 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
+ * @brief computer vision thread
+ *
+ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
+ */
+
+
+#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);