move computervision to modules

sw/airborne/modules/computer_vision/OpticFlow/OpticFlowHover.xml

@@ -0,0 +1,44 @@
+<!DOCTYPE module SYSTEM "../module.dtd">
+
+<module name="OpticFlow">
+  <doc>
+    <description>Video ARDone 2</description>
+  </doc>
+
+  <header>
+    <file name="opticflow_module.h"/>
+  </header>
+
+  <init fun="opticflow_module_init()"/>
+
+  <periodic fun="opticflow_module_run()" freq="60" start="opticflow_module_start()" stop="opticflow_module_stop()" autorun="TRUE"/>
+  <makefile>
+    <define name="ARDRONE_VIDEO_PORT" value="2002" />
+    <define name="USE_ARDRONE_VIDEO" />
+      <raw>
+
+include $(PAPARAZZI_HOME)/sw/ext/ardrone2_vision/Makefile.paths
+          
+VISION_MODULE_FOLDER = $(DIR_MODULES)/OpticFlow
+      	     	
+$(TARGET).CFLAGS += -I$(DIR_MODULES) -I$(DIR_CV) -I$(DIR_LIB) -pthread -D__USE_GNU
+$(TARGET).CXXFLAGS += -I$(PAPARAZZI_HOME)/sw/include/ -I$(PAPARAZZI_SRC)/sw/airborne -I$(PAPARAZZI_SRC)/conf/autopilot -I$(PAPARAZZI_SRC)/sw/airborne/arch/$($(TARGET).ARCHDIR) -I$(VARINCLUDE) -I$(ACINCLUDE) -I$(PAPARAZZI_SRC)/sw/airborne/modules/
+$(TARGET).CXXFLAGS += -I$(DIR_MODULES) -I$(DIR_CV) -I$(DIR_LIB) -pthread -D__USE_GNU
+
+$(TARGET).srcs += $(VISION_MODULE_FOLDER)/opticflow_module.c
+$(TARGET).srcs += $(VISION_MODULE_FOLDER)/opticflow_code.c
+$(TARGET).srcs += $(VISION_MODULE_FOLDER)/hover_stabilization.c
+$(TARGET).srcs += $(DIR_CV)/opticflow/optic_flow_ardrone.c
+$(TARGET).srcs += $(DIR_CV)/opticflow/fast9/fastRosten.c
+$(TARGET).srcs += $(DIR_CV)/encoding/jpeg.c
+$(TARGET).srcs += $(DIR_CV)/encoding/rtp.c
+$(TARGET).srcs += $(DIR_CV)/trig.c
+$(TARGET).srcs += $(DIR_LIB)/udp/socket.c
+$(TARGET).srcs += $(DIR_LIB)/v4l/video.c
+$(TARGET).CFLAGS += -I$(DIR_MODULES) -I$(DIR_CV) -I$(DIR_LIB) -pthread
+$(TARGET).LDFLAGS += -pthread -lrt -static
+
+    </raw>
+  </makefile>
+</module>
+

sw/airborne/modules/computer_vision/OpticFlow/dummy.c

@@ -0,0 +1,3 @@
+void dummyFunction(void) {
+	return;
+}

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

@@ -0,0 +1,166 @@
+/*
+ * Copyright (C) 2014 Hann Woei Ho
+ *
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/*
+ * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/hover_stabilization.c
+ * @brief optical-flow based hovering for Parrot AR.Drone 2.0
+ *
+ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
+ */
+
+// Own Header
+#include "hover_stabilization.h"
+
+// Vision Data
+#include "opticflow_code.h"
+
+// Stabilization
+//#include "stabilization.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
+#include "autopilot.h"
+
+// Downlink
+#include "subsystems/datalink/downlink.h"
+
+// Controller Gains
+/* error if some gains are negative */
+#if (VISION_PHI_PGAIN < 0)      ||                   \
+  (VISION_PHI_IGAIN < 0)        ||                   \
+  (VISION_THETA_PGAIN < 0)      ||                   \
+  (VISION_THETA_IGAIN < 0)
+#error "ALL control gains have to be positive!!!"
+#endif
+bool activate_opticflow_hover;
+float vision_desired_vx;
+float vision_desired_vy;
+int32_t vision_phi_pgain;
+int32_t vision_phi_igain;
+int32_t vision_theta_pgain;
+int32_t vision_theta_igain;
+
+// Controller Commands
+struct Int32Eulers cmd_euler;
+
+// Hover Stabilization
+float Velx_Int;
+float Vely_Int;
+float Error_Velx;
+float Error_Vely;
+
+#define CMD_OF_SAT	1500 // 40 deg = 2859.1851
+unsigned char saturateX = 0, saturateY = 0;
+unsigned int set_heading;
+
+void init_hover_stabilization_onvision()
+{
+	INT_EULERS_ZERO(cmd_euler);
+
+	activate_opticflow_hover = VISION_HOVER;
+	vision_phi_pgain = VISION_PHI_PGAIN;
+	vision_phi_igain = VISION_PHI_IGAIN;
+	vision_theta_pgain = VISION_THETA_PGAIN;
+	vision_theta_igain = VISION_THETA_IGAIN;
+	vision_desired_vx = VISION_DESIRED_VX;
+	vision_desired_vy = VISION_DESIRED_VY;
+
+	set_heading = 1;
+
+	Error_Velx = 0;
+	Error_Vely = 0;
+	Velx_Int = 0;
+	Vely_Int = 0;
+}
+
+void run_hover_stabilization_onvision(void)
+{
+	if(autopilot_mode == AP_MODE_VISION_HOVER)
+	{
+		run_opticflow_hover();
+	}
+	else
+	{
+		Velx_Int = 0;
+		Vely_Int = 0;
+	}
+}
+
+void run_opticflow_hover(void)
+{
+	if(flow_count)
+	{
+		Error_Velx = Velx - vision_desired_vx;
+		Error_Vely = Vely - vision_desired_vy;
+	}
+	else
+	{
+		Error_Velx = 0;
+		Error_Vely = 0;
+	}
+
+	if(saturateX==0)
+	{
+		if(activate_opticflow_hover==TRUE)
+		{
+			Velx_Int += vision_theta_igain*Error_Velx;
+		}
+		else
+		{
+			Velx_Int += vision_theta_igain*V_body.x;
+		}
+	}
+	if(saturateY==0)
+	{
+		if(activate_opticflow_hover==TRUE)
+		{
+			Vely_Int += vision_phi_igain*Error_Vely;
+		}
+		else
+		{
+			Vely_Int += vision_phi_igain*V_body.y;
+		}
+	}
+
+	if(set_heading)
+	{
+		cmd_euler.psi = stateGetNedToBodyEulers_i()->psi;
+		set_heading = 0;
+	}
+
+	if(activate_opticflow_hover==TRUE)
+	{
+		cmd_euler.phi =  - (vision_phi_pgain*Error_Vely + Vely_Int);
+		cmd_euler.theta =  (vision_theta_pgain*Error_Velx + Velx_Int);
+	}
+	else
+	{
+		cmd_euler.phi =  - (vision_phi_pgain*V_body.y + Vely_Int);
+		cmd_euler.theta =  (vision_theta_pgain*V_body.x + Velx_Int);
+	}
+
+	saturateX = 0; saturateY = 0;
+	if(cmd_euler.phi<-CMD_OF_SAT){cmd_euler.phi = -CMD_OF_SAT; saturateX = 1;}
+	else if(cmd_euler.phi>CMD_OF_SAT){cmd_euler.phi = CMD_OF_SAT; saturateX = 1;}
+	if(cmd_euler.theta<-CMD_OF_SAT){cmd_euler.theta = -CMD_OF_SAT; saturateY = 1;}
+	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, &Vely_Int, &cmd_euler.phi, &cmd_euler.theta);
+}

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

@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2014 Hann Woei Ho
+ *
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/*
+ * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/hover_stabilization.h
+ * @brief optical-flow based hovering for Parrot AR.Drone 2.0
+ *
+ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
+ */
+
+#ifndef HOVER_STABILIZATION_H_
+#define HOVER_STABILIZATION_H_
+
+#include <std.h>
+
+void init_hover_stabilization_onvision(void);
+void run_hover_stabilization_onvision(void);
+void run_opticflow_hover(void);
+
+extern bool activate_opticflow_hover;
+extern float vision_desired_vx;
+extern float vision_desired_vy;
+extern int32_t vision_phi_pgain;
+extern int32_t vision_phi_igain;
+extern int32_t vision_theta_pgain;
+extern int32_t vision_theta_igain;
+
+#endif /* HOVER_STABILIZATION_H_ */

sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.c

@@ -0,0 +1,332 @@
+/*
+ * Copyright (C) 2014 Hann Woei Ho
+ *
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/*
+ * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_code.c
+ * @brief optical-flow based hovering for Parrot AR.Drone 2.0
+ *
+ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+// Own Header
+#include "opticflow_code.h"
+
+// Computer Vision
+#include "opticflow/optic_flow_gdc.h"
+#include "opticflow/fast9/fastRosten.h"
+#include "opticflow_module.h"
+
+// Paparazzi Data
+#include "subsystems/ins/ins_int.h"
+#include "subsystems/imu.h"
+
+// Downlink
+#include "subsystems/datalink/downlink.h"
+
+// Timer
+#include <sys/time.h>
+
+// Settable by plugin
+unsigned int imgWidth, imgHeight;
+unsigned int verbose = 0;
+
+// 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
+#define FOV_W 0.89360857702
+#define Fx_ARdrone 343.1211
+#define Fy_ARdrone 348.5053
+
+// Corner Detection
+int *x, *y;
+int count = 0;
+int max_count = 25;
+#define MAX_COUNT 100
+
+// Corner Tracking
+int *new_x, *new_y, *status, *dx, *dy;
+int error_opticflow;
+int flow_count = 0;
+int remove_point;
+int c;
+int borderx = 24, bordery = 24;
+
+// Remove bad corners
+float distance2, min_distance, min_distance2;
+
+// Flow Derotation
+#define FLOW_DEROTATION
+float curr_pitch, curr_roll, curr_yaw, prev_pitch, prev_roll;
+float cam_h, diff_roll, diff_pitch, OFx_trans, OFy_trans;
+
+// Lateral Velocity Computation
+float Velx, Vely;
+
+// Compute body velocities
+struct FloatVect3 V_Ned;
+struct FloatRMat Rmat_Ned2Body;
+struct FloatVect3 V_body;
+
+// Called by plugin
+void my_plugin_init(void)
+{
+	// Initialize variables
+	gray_frame = (unsigned char *) calloc(imgWidth*imgHeight,sizeof(unsigned char));
+	prev_frame = (unsigned char *) calloc(imgWidth*imgHeight*2,sizeof(unsigned char));
+	prev_gray_frame = (unsigned char *) calloc(imgWidth*imgHeight,sizeof(unsigned char));
+	x = (int *) calloc(MAX_COUNT,sizeof(int));
+	new_x = (int *) calloc(MAX_COUNT,sizeof(int));
+	y = (int *) calloc(MAX_COUNT,sizeof(int));
+	new_y = (int *) calloc(MAX_COUNT,sizeof(int));
+	status = (int *) calloc(MAX_COUNT,sizeof(int));
+	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;
+	prev_pitch = 0.0;
+	prev_roll = 0.0;
+	curr_pitch = 0.0;
+	curr_roll = 0.0;
+	curr_yaw = 0.0;
+	OFx_trans = 0.0;
+	OFy_trans = 0.0;
+	Velx = 0.0;
+	Vely = 0.0;
+}
+
+void my_plugin_run(unsigned char *frame)
+{
+	if(old_img_init == 1)
+	{
+		memcpy(prev_frame,frame,imgHeight*imgWidth*2);
+		CvtYUYV2Gray(prev_gray_frame, prev_frame, imgWidth, imgHeight);
+		old_img_init = 0;
+	}
+
+	// ***********************************************************************************************************************
+	// Additional information from other sensors
+	// ***********************************************************************************************************************
+
+    // Compute body velocities from ENU
+    V_Ned.x = stateGetSpeedNed_f()->x;
+    V_Ned.y = stateGetSpeedNed_f()->y;
+    V_Ned.z = stateGetSpeedNed_f()->z;
+
+    struct FloatQuat* BodyQuaternions = stateGetNedToBodyQuat_f();
+    FLOAT_RMAT_OF_QUAT(Rmat_Ned2Body,*BodyQuaternions);
+    RMAT_VECT3_MUL(V_body, Rmat_Ned2Body, V_Ned);
+
+	// ***********************************************************************************************************************
+	// Corner detection
+	// ***********************************************************************************************************************
+
+	// FAST corner detection
+	int fast_threshold = 20;
+	xyFAST* pnts_fast;
+	pnts_fast = fast9_detect((const byte*)prev_gray_frame, imgWidth, imgHeight, imgWidth, fast_threshold, &count);
+
+	if(count > MAX_COUNT) count = MAX_COUNT;
+	for(int i = 0; i < count; i++)
+	{
+		x[i] = pnts_fast[i].x;
+		y[i] = pnts_fast[i].y;
+	}
+	free(pnts_fast);
+
+	// Remove neighbouring corners
+	min_distance = 3;
+	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++)
+		{
+			// distance squared:
+			distance2 = (x[i] - x[j])*(x[i] - x[j]) + (y[i] - y[j])*(y[i] - y[j]);
+			if(distance2 < min_distance2)
+			{
+				labelmin[i] = 1;
+			}
+		}
+	}
+
+	int count_fil = count;
+	for(int i = count-1; i >= 0; i-- )
+	{
+		remove_point = 0;
+
+		if(labelmin[i])
+		{
+			remove_point = 1;
+		}
+
+		if(remove_point)
+		{
+			for(c = i; c <count_fil-1; c++)
+			{
+				x[c] = x[c+1];
+				y[c] = y[c+1];
+			}
+			count_fil--;
+		}
+	}
+
+	if(count_fil>max_count) count_fil = max_count;
+	count = count_fil;
+	free(labelmin);
+
+	// **********************************************************************************************************************
+	// Corner Tracking
+	// **********************************************************************************************************************
+	CvtYUYV2Gray(gray_frame, frame, imgWidth, imgHeight);
+
+	error_opticflow = opticFlowLK(gray_frame, prev_gray_frame, x, y, count_fil, imgWidth, imgHeight, new_x, new_y, status, 5, 100);
+
+	flow_count = count_fil;
+	for(int i=count_fil-1; i>=0; i--)
+	{
+		remove_point = 1;
+
+		if(status[i] && !(new_x[i] < borderx || new_x[i] > (imgWidth-1-borderx) ||
+				new_y[i] < bordery || new_y[i] > (imgHeight-1-bordery)))
+		{
+			remove_point = 0;
+		}
+
+		if(remove_point)
+		{
+			for(c = i; c <flow_count-1; c++)
+			{
+				x[c] = x[c+1];
+				y[c] = y[c+1];
+				new_x[c] = new_x[c+1];
+				new_y[c] = new_y[c+1];
+			}
+			flow_count--;
+		}
+	}
+
+	dx_sum = 0.0;
+	dy_sum = 0.0;
+
+	// Optical Flow Computation
+	for(int i=0; i<flow_count; i++)
+	{
+		dx[i] = new_x[i] - x[i];
+		dy[i] = new_y[i] - y[i];
+	}
+
+	// Median Filter
+	if(flow_count)
+	{
+		quick_sort_int(dx,flow_count); // 11
+		quick_sort_int(dy,flow_count); // 11
+
+		dx_sum = (float) dx[flow_count/2];
+		dy_sum = (float) dy[flow_count/2];
+	}
+	else
+	{
+		dx_sum = 0.0;
+		dy_sum = 0.0;
+	}
+
+	// Flow Derotation
+	curr_pitch = stateGetNedToBodyEulers_f()->theta;
+	curr_roll = stateGetNedToBodyEulers_f()->phi;
+	curr_yaw = stateGetNedToBodyEulers_f()->psi;
+
+	diff_pitch = (curr_pitch - prev_pitch)*imgHeight/FOV_H;
+	diff_roll = (curr_roll - prev_roll)*imgWidth/FOV_W;
+
+	prev_pitch = curr_pitch;
+	prev_roll = curr_roll;
+
+#ifdef FLOW_DEROTATION
+	if(flow_count)
+	{
+		OFx_trans = dx_sum - diff_roll;
+		OFy_trans = dy_sum - diff_pitch;
+
+		if((OFx_trans<=0) != (dx_sum<=0))
+		{
+			OFx_trans = 0;
+			OFy_trans = 0;
+		}
+	}
+	else
+	{
+		OFx_trans = dx_sum;
+		OFy_trans = dy_sum;
+	}
+#else
+	OFx_trans = dx_sum;
+	OFy_trans = dy_sum;
+#endif
+
+	// Average Filter
+	OFfilter(&OFx, &OFy, OFx_trans, OFy_trans, flow_count, 1);
+
+	// Velocity Computation
+	#ifdef USE_SONAR
+		cam_h = ins_impl.sonar_z;
+	#else
+		cam_h = 1;
+	#endif
+
+	if(flow_count)
+	{
+		Velx = OFy*cam_h*FPS/Fy_ARdrone + 0.05;
+		Vely = -OFx*cam_h*FPS/Fx_ARdrone - 0.1;
+	}
+	else
+	{
+		Velx = 0.0;
+		Vely = 0.0;
+	}
+
+	// **********************************************************************************************************************
+	// Next Loop Preparation
+	// **********************************************************************************************************************
+
+	memcpy(prev_frame,frame,imgHeight*imgWidth*2);
+	memcpy(prev_gray_frame,gray_frame,imgHeight*imgWidth);
+
+	// **********************************************************************************************************************
+	// 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);
+}
+

sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.h

@@ -0,0 +1,49 @@
+/*
+ * Copyright (C) 2014 Hann Woei Ho
+ *
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/*
+ * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_code.h
+ * @brief optical-flow based hovering for Parrot AR.Drone 2.0
+ *
+ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
+ */
+
+#ifndef _OPT_FL_LAND_H
+#define _OPT_FL_LAND_H
+
+// Settable by pluging
+extern unsigned int imgWidth, imgHeight;
+extern unsigned int verbose;
+
+// Variables used by the controller
+extern float Velx, Vely;
+extern int count;
+extern int flow_count;
+extern struct FloatVect3 V_body;
+
+// Called by plugin
+void my_plugin_init(void);
+void my_plugin_run(unsigned char *frame);
+
+// Timer
+void start_timer_rates(void);
+long end_timer_rates(void);
+#endif

sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.c

@@ -0,0 +1,218 @@
+/*
+ * Copyright (C) 2014 Hann Woei Ho
+ *
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_module.h
+ * @brief optical-flow based hovering for Parrot AR.Drone 2.0
+ *
+ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
+ */
+
+
+// Own header
+#include "opticflow_module.h"
+
+// Navigate Based On Vision
+#include "hover_stabilization.h"
+
+// Paparazzi
+#include "state.h" // for attitude
+#include "boards/ardrone/navdata.h" // for ultrasound Height
+
+// Threaded computer vision
+#include <pthread.h>
+
+// Frame Rate (FPS)
+#include <sys/time.h>
+float FPS;
+volatile long timestamp;
+#define USEC_PER_SEC 1000000L
+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;
+}
+
+struct timeval start_time;
+struct timeval end_time;
+
+void start_timer() {
+	gettimeofday (&start_time, NULL);
+}
+long end_timer() {
+	gettimeofday (&end_time, NULL);
+	return time_elapsed(&start_time, &end_time);
+}
+
+void opticflow_module_init(void)
+{
+	// Immediately start the vision thread when the module initialized
+	opticflow_module_start();
+
+	// Stabilization Code Initialization
+	init_hover_stabilization_onvision();
+
+	// Frame Rate Initialization
+	FPS = 0.0;
+	timestamp=0;
+	start_timer();
+}
+
+
+volatile uint8_t computervision_thread_has_results = 0;
+
+void opticflow_module_run(void)
+{
+
+  // Read Latest Vision Module Results
+  if (computervision_thread_has_results)
+  {
+    computervision_thread_has_results = 0;
+    run_hover_stabilization_onvision();
+  }
+}
+
+/////////////////////////////////////////////////////////////////////////
+// COMPUTER VISION THREAD
+
+// Video
+#include "v4l/video.h"
+#include "resize.h"
+
+// Payload Code
+#include "opticflow_code.h"
+
+// Downlink Video
+//#define DOWNLINK_VIDEO 1
+
+#ifdef DOWNLINK_VIDEO
+#include "encoding/jpeg.h"
+#include "encoding/rtp.h"
+#endif
+
+#include <stdio.h>
+
+pthread_t computervision_thread;
+volatile uint8_t computervision_thread_status = 0;
+volatile uint8_t computer_vision_thread_command = 0;
+void *computervision_thread_main(void* data);
+void *computervision_thread_main(void* 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;
+
+	if (video_init(&vid)<0) {
+		printf("Error initialising video\n");
+		computervision_thread_status = -1;
+		return 0;
+	}
+
+	// Video Grabbing
+	struct img_struct* img_new = video_create_image(&vid);
+
+	// Video Resizing
+	#define DOWNSIZE_FACTOR 1
+	struct img_struct small;
+	small.w = vid.w / DOWNSIZE_FACTOR;
+	small.h = vid.h / DOWNSIZE_FACTOR;
+	small.buf = (uint8_t*)malloc(small.w*small.h*2);
+
+#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
+	imgWidth = small.w;
+	imgHeight = small.h;
+	verbose = 2;
+	my_plugin_init();
+
+	while (computer_vision_thread_command > 0)
+	{
+		video_grab_image(&vid, img_new);
+
+		// FPS
+		timestamp = end_timer();
+		FPS = (float) 1000000/(float)timestamp;
+		//	printf("dt = %d, FPS = %f\n",timestamp, FPS);
+		start_timer();
+
+		// Resize
+		//resize_uyuv(img_new, &small, DOWNSIZE_FACTOR);
+
+		// Run Image Processing
+		my_plugin_run(img_new->buf);
+		//	my_plugin_run(small.buf);
+
+		#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
+		computervision_thread_has_results++;
+	}
+	printf("Thread Closed\n");
+	video_close(&vid);
+	computervision_thread_status = -100;
+	return 0;
+}
+
+void opticflow_module_start(void)
+{
+	computer_vision_thread_command = 1;
+	int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main, NULL);
+	if(rc) {
+		printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc);
+	}
+}
+
+void opticflow_module_stop(void)
+{
+	computer_vision_thread_command = 0;
+}
+
+
+

sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.h

@@ -0,0 +1,45 @@
+/*
+ * Copyright (C) 2014 Hann Woei Ho
+ *
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/*
+ * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_module.h
+ * @brief optical-flow based hovering for Parrot AR.Drone 2.0
+ *
+ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
+ */
+
+#ifndef OPTICFLOW_LAND_H
+#define OPTICFLOW_LAND_H
+
+// Module functions
+extern void opticflow_module_init(void);
+extern void opticflow_module_run(void);
+extern void opticflow_module_start(void);
+extern void opticflow_module_stop(void);
+
+// Frame Rate
+extern float FPS;
+struct timeval;
+long time_elapsed(struct timeval *t1, struct timeval *t2);
+void start_timer(void);
+long end_timer(void);
+
+#endif /* OPTICFLOW_LAND_H */

sw/airborne/modules/computer_vision/OpticFlow/video_message_structs.h

@@ -0,0 +1,32 @@
+#ifndef VMSTRTS_H
+#define VMSTRTS_H
+
+/***
+      an exact copy of this file to exist in ppz
+      this files keeps the structs that are serialized and streamed over tcp/ip through localhost
+ */
+
+#define N_BINS  10
+
+struct gst2ppz_message_struct
+{
+  unsigned int ID;                // Keep different modules for using each others data
+  unsigned int counter;		        // counter to keep track of data
+  unsigned int obstacle_bins[N_BINS];		  // optical flow output, shift in x direction
+  unsigned int uncertainty_bins[N_BINS];		//optical flow output, shift in y direction
+};
+extern struct gst2ppz_message_struct gst2ppz;
+
+struct ppz2gst_message_struct
+{
+  unsigned int ID;                // Keep different modules for using each others data
+  unsigned int counter;		        //counter to keep track of data
+  int pitch;
+  int roll;
+  int alt;
+  int adjust_factor;              // 0-10 :adjust brightness
+};
+extern struct ppz2gst_message_struct ppz2gst;
+
+#endif  /*  VMSTRTS_H  */
+

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

@@ -40,10 +40,8 @@ inline void grayscale_uyvy(struct img_struct *input, struct img_struct *output)
   }
 }
 
-inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m,
-                          uint8_t u_M, uint8_t v_m, uint8_t v_M);
-inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m,
-                          uint8_t u_M, uint8_t v_m, uint8_t v_M)
+inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M);
+inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M)
 {
   int cnt = 0;
   uint8_t *source = input->buf;

sw/airborne/modules/computer_vision/cv/encoding/jpeg.c

@@ -433,8 +433,7 @@ void MakeTables(int q)
 
 
 
-uint8_t *encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_factor, uint32_t image_format,
-                      uint32_t image_width, uint32_t image_height, uint8_t add_dri_header)
+uint8_t *encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_factor, uint32_t image_format, uint32_t image_width, uint32_t image_height, uint8_t add_dri_header)
 {
   uint16_t i, j;
 

sw/airborne/modules/computer_vision/cv/encoding/rtp.c

@@ -7,9 +7,7 @@
 
 #include "rtp.h"
 
-void send_rtp_packet(struct UdpSocket *sock, uint8_t *Jpeg, int JpegLen, uint32_t m_SequenceNumber,
-                     uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code,
-                     uint8_t has_dri_header);
+void send_rtp_packet(struct UdpSocket *sock, uint8_t *Jpeg, int JpegLen, uint32_t m_SequenceNumber, uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header);
 
 // http://www.ietf.org/rfc/rfc3550.txt
 
@@ -51,19 +49,16 @@ void test_rtp_frame(struct UdpSocket *sock)
   uint8_t quality_code = 0x54;
 
   if (toggle) {
-    send_rtp_packet(sock, JpegScanDataCh2A, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code,
-                    quality_code, 0);
+    send_rtp_packet(sock, JpegScanDataCh2A, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0);
   } else {
-    send_rtp_packet(sock, JpegScanDataCh2B, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code,
-                    quality_code, 0);
+    send_rtp_packet(sock, JpegScanDataCh2B, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0);
   }
   framecounter++;
   timecounter += 3600;
 }
 
 
-void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int w, int h, uint8_t format_code,
-                    uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t)
+void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t)
 {
   static uint32_t packetcounter = 0;
   static uint32_t timecounter = 0;
@@ -88,8 +83,7 @@ void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int
       len = JpegLen;
     }
 
-    send_rtp_packet(sock, Jpeg, len, packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code,
-                    has_dri_header);
+    send_rtp_packet(sock, Jpeg, len, packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code, has_dri_header);
 
     JpegLen   -= len;
     Jpeg      += len;

sw/airborne/modules/computer_vision/cv/opticflow/fast9/LICENSE

@@ -0,0 +1,30 @@
+Copyright (c) 2006, 2008 Edward Rosten
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+
+	*Redistributions of source code must retain the above copyright
+	 notice, this list of conditions and the following disclaimer.
+
+	*Redistributions in binary form must reproduce the above copyright
+	 notice, this list of conditions and the following disclaimer in the
+	 documentation and/or other materials provided with the distribution.
+
+	*Neither the name of the University of Cambridge nor the names of
+	 its contributors may be used to endorse or promote products derived
+	 from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.h

@@ -0,0 +1,51 @@
+/*
+Copyright (c) 2006, 2008 Edward Rosten
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+
+	*Redistributions of source code must retain the above copyright
+	 notice, this list of conditions and the following disclaimer.
+
+	*Redistributions in binary form must reproduce the above copyright
+	 notice, this list of conditions and the following disclaimer in the
+	 documentation and/or other materials provided with the distribution.
+
+	*Neither the name of the University of Cambridge nor the names of
+	 its contributors may be used to endorse or promote products derived
+	 from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef FAST_H
+#define FAST_H
+
+typedef struct { int x, y; } xyFAST; 
+typedef unsigned char byte;
+
+int fast9_corner_score(const byte* p, const int pixel[], int bstart);
+
+xyFAST* fast9_detect(const byte* im, int xsize, int ysize, int stride, int b, int* ret_num_corners);
+
+int* fast9_score(const byte* i, int stride, xyFAST* corners, int num_corners, int b);
+
+xyFAST* fast9_detect_nonmax(const byte* im, int xsize, int ysize, int stride, int b, int* ret_num_corners);
+
+xyFAST* nonmax_suppression(const xyFAST* corners, const int* scores, int num_corners, int* ret_num_nonmax);
+
+
+#endif

sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h

@@ -0,0 +1,43 @@
+/*
+ * Copyright (C) 2014 Hann Woei Ho
+ *
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/*
+ * @file paparazzi/sw/ext/ardrone2_vision/cv/opticflow/optic_flow_gdc.h
+ * @brief optical-flow based hovering for Parrot AR.Drone 2.0
+ *
+ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
+ */
+
+#ifndef OPTIC
+#define OPTIC
+void multiplyImages(int *ImA, int *ImB, int *ImC, int width, int height);
+void getImageDifference(int *ImA, int *ImB, int *ImC, int width, int height);
+void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int center_y, int half_window_size, int subpixel_factor);
+void getGradientPatch(int *Patch, int *DX, int *DY, int half_window_size);
+int getSumPatch(int *Patch, int size);
+int calculateG(int *G, int *DX, int *DY, int half_window_size);
+int calculateError(int *ImC, int width, int height);
+int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int *p_x, int *p_y, int n_found_points, int imW, int imH, int *new_x, int *new_y, int *status, int half_window_size, int max_iterations);
+void quick_sort(float *a, int n);
+void quick_sort_int(int *a, int n);
+void CvtYUYV2Gray(unsigned char *grayframe, unsigned char *frame, int imW, int imH);
+void OFfilter(float *OFx, float *OFy, float dx, float dy, int count, int OF_FilterType);
+#endif

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

@@ -36,13 +36,13 @@ inline void resize_uyuv(struct img_struct *input, struct img_struct *output, int
       *dest++ = *source++; // U
       *dest++ = *source++; // Y
       // now skip 3 pixels
-      if (pixelskip) { source += (pixelskip + 1) * 2; }
+      source += (pixelskip + 1) * 2;
       *dest++ = *source++; // U
       *dest++ = *source++; // V
-      if (pixelskip) { source += (pixelskip - 1) * 2; }
+      source += (pixelskip - 1) * 2;
     }
     // skip 3 rows
-    if (pixelskip) { source += pixelskip * input->w * 2; }
+    source += pixelskip * input->w * 2;
   }
 }
 

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

@@ -0,0 +1,158 @@
+/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+ * Trigonometry
+ * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+
+#include "trig.h"
+
+static int cosine[] = {
+  10000, 9998, 9994, 9986, 9976, 9962, 9945, 9925, 9903, 9877,
+  9848, 9816, 9781, 9744, 9703, 9659, 9613, 9563, 9511, 9455,
+  9397, 9336, 9272, 9205, 9135, 9063, 8988, 8910, 8829, 8746,
+  8660, 8572, 8480, 8387, 8290, 8192, 8090, 7986, 7880, 7771,
+  7660, 7547, 7431, 7314, 7193, 7071, 6947, 6820, 6691, 6561,
+  6428, 6293, 6157, 6018, 5878, 5736, 5592, 5446, 5299, 5150,
+  5000, 4848, 4695, 4540, 4384, 4226, 4067, 3907, 3746, 3584,
+  3420, 3256, 3090, 2924, 2756, 2588, 2419, 2250, 2079, 1908,
+  1736, 1564, 1392, 1219, 1045,  872,  698,  523,  349,  175,
+  0
+};
+
+int sin_zelf(int ix)
+{
+  while (ix < 0) {
+    ix = ix + 360;
+  }
+  while (ix >= 360) {
+    ix = ix - 360;
+  }
+  if (ix < 90) { return cosine[90 - ix] / 10; }
+  if (ix < 180) { return cosine[ix - 90] / 10; }
+  if (ix < 270) { return -cosine[270 - ix] / 10; }
+  if (ix < 360) { return -cosine[ix - 270] / 10; }
+  return 0;
+}
+
+int cos_zelf(int ix)
+{
+  while (ix < 0) {
+    ix = ix + 360;
+  }
+  while (ix >= 360) {
+    ix = ix - 360;
+  }
+  if (ix < 90) { return cosine[ix] / 10; }
+  if (ix < 180) { return -cosine[180 - ix] / 10; }
+  if (ix < 270) { return -cosine[ix - 180] / 10; }
+  if (ix < 360) { return cosine[360 - ix] / 10; }
+  return 0;
+}
+
+int tan_zelf(int ix)
+{
+
+  while (ix < 0) {
+    ix = ix + 360;
+  }
+  while (ix >= 360) {
+    ix = ix - 360;
+  }
+  if (ix == 90) { return 9999; }
+  if (ix == 270) { return -9999; }
+  if (ix < 90) { return (1000 * cosine[90 - ix]) / cosine[ix]; }
+  if (ix < 180) { return -(1000 * cosine[ix - 90]) / cosine[180 - ix]; }
+  if (ix < 270) { return (1000 * cosine[270 - ix]) / cosine[ix - 180]; }
+  if (ix < 360) { return -(1000 * cosine[ix - 270]) / cosine[360 - ix]; }
+  return 0;
+}
+
+int asin_zelf(int y, int hyp)
+{
+  int quot, sgn, ix;
+  if ((y > hyp) || (y == 0)) {
+    return 0;
+  }
+  sgn = hyp * y;
+  if (hyp < 0) {
+    hyp = -hyp;
+  }
+  if (y < 0) {
+    y = -y;
+  }
+  quot = (y * 10000) / hyp;
+  if (quot > 9999) {
+    quot = 9999;
+  }
+  for (ix = 0; ix < 90; ix++)
+    if ((quot < cosine[ix]) && (quot >= cosine[ix + 1])) {
+      break;
+    }
+  if (sgn < 0) {
+    return -(90 - ix);
+  } else {
+    return 90 - ix;
+  }
+}
+
+int acos_zelf(int x, int hyp)
+{
+  int quot, sgn, ix;
+  if (x > hyp) {
+    return 0;
+  }
+  if (x == 0) {
+    if (hyp < 0) {
+      return -90;
+    } else {
+      return 90;
+    }
+    return 0;
+  }
+  sgn = hyp * x;
+  if (hyp < 0) {
+    hyp = -hyp;
+  }
+  if (x < 0) {
+    x = -x;
+  }
+  quot = (x * 10000) / hyp;
+  if (quot > 9999) {
+    quot = 9999;
+  }
+  for (ix = 0; ix < 90; ix++)
+    if ((quot < cosine[ix]) && (quot >= cosine[ix + 1])) {
+      break;
+    }
+  if (sgn < 0) {
+    return -ix;
+  } else {
+    return ix;
+  }
+}
+
+//atan_zelf(y/x) in degrees
+int atan_zelf(int y, int x)
+{
+  int angle, flip, t, xy;
+
+  if (x < 0) { x = -x; }
+  if (y < 0) { y = -y; }
+  flip = 0;  if (x < y) { flip = 1; t = x;  x = y;  y = t; }
+  if (x == 0) { return 90; }
+
+  xy = (y * 1000) / x;
+  angle = (360 * xy) / (6283 + ((((1764 * xy) / 1000) * xy) / 1000));
+  if (flip) { angle = 90 - angle; }
+  return angle;
+}
+
+unsigned int isqrt(unsigned int val)
+{
+  unsigned int temp, g = 0, b = 0x8000, bshft = 15;
+  do {
+    if (val >= (temp = (((g << 1) + b) << bshft--))) {
+      g += b;
+      val -= temp;
+    }
+  } while (b >>= 1);
+  return g;
+}

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

@@ -0,0 +1,7 @@
+int cos_zelf(int ix);
+int sin_zelf(int);
+int tan_zelf(int);
+int acos_zelf(int, int);
+int asin_zelf(int, int);
+int atan_zelf(int, int);
+unsigned int isqrt(unsigned int);

sw/airborne/modules/computer_vision/lib/paparazzi.h

@@ -0,0 +1,32 @@
+
+////////////////////////////////////////////////
+// Paparazzi communication interface
+
+#include <pthread.h>    // pthread_create
+#include <gst/gst.h>    // gprint
+
+#include "socket.h"
+#include "video_message_structs.h"
+
+struct gst2ppz_message_struct gst2ppz;
+struct ppz2gst_message_struct ppz2gst;
+
+inline void paparazzi_message_server_start(void);
+inline void paparazzi_message_send(void);
+
+struct UdpSocket *sock;
+
+inline void paparazzi_message_server_start(void)
+{
+  sock = udp_socket("192.168.1.1", 2000, 2001, FMS_UNICAST);
+}
+
+
+inline void paparazzi_message_send(void)
+{
+  udp_write(sock, (char *) &gst2ppz, sizeof(gst2ppz));
+  int ret = udp_read(sock, (unsigned char *) &ppz2gst, sizeof(ppz2gst));
+  printf("read: %d \n",ret);
+}
+
+

sw/airborne/modules/computer_vision/lib/tcp/socket.c

@@ -0,0 +1,139 @@
+#include "socket.h"
+#include <sys/socket.h>       /*  socket definitions        */
+#include <sys/types.h>        /*  socket types              */
+#include <arpa/inet.h>        /*  inet (3) funtions         */
+#include <unistd.h>           /*  misc. UNIX functions      */
+#include <errno.h>
+#include <string.h>     /* memset */
+
+#include <stdlib.h>
+#include <stdio.h>
+
+
+
+
+/*  Global constants  */
+
+#define ECHO_PORT          (2002)
+#define MAX_LINE           (1000)
+
+/*  Global variables  */
+int       list_s;                /*  listening socket          */
+int       conn_s;                /*  connection socket         */
+struct    sockaddr_in servaddr;  /*  socket address structure  */
+char      buffer[MAX_LINE];      /*  character buffer          */
+char     *endptr;                /*  for strtol()              */
+
+int closeSocket(void)
+{
+  return close(conn_s);
+}
+
+int initSocket(void)
+{
+
+  /*  Create the listening socket  */
+
+  if ((list_s = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
+    fprintf(stderr, "ECHOSERV: Error creating listening socket.\n");
+    return -1;
+  }
+
+
+  /*  Set all bytes in socket address structure to
+        zero, and fill in the relevant data members   */
+
+  memset(&servaddr, 0, sizeof(servaddr));
+  servaddr.sin_family      = AF_INET;
+  servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
+  servaddr.sin_port        = htons(ECHO_PORT);
+
+  /*  Bind our socket addresss to the
+  listening socket, and call listen()  */
+
+  if (bind(list_s, (struct sockaddr *) &servaddr, sizeof(servaddr)) < 0) {
+    fprintf(stderr, "ECHOSERV: Error calling bind()\n");
+    exit(EXIT_FAILURE);
+  }
+
+  if (listen(list_s, LISTENQ) < 0) {
+    fprintf(stderr, "ECHOSERV: Error calling listen()\n");
+    exit(EXIT_FAILURE);
+  }
+
+
+
+  /*  Wait for a connection, then accept() it  */
+  if ((conn_s = accept(list_s, NULL, NULL)) < 0) {
+    fprintf(stderr, "ECHOSERV: Error calling function accept()\n");
+    return -1;
+  }
+  printf("Connected!\n");
+  return 1;
+}
+
+/*  Read a line from a socket  */
+
+ssize_t Readline_socket(void *vptr, size_t maxlen)
+{
+  ssize_t n, rc;
+  char    c, *buffer;
+
+  buffer = vptr;
+
+  for (n = 1; n < maxlen; n++) {
+
+    if ((rc = read(conn_s, &c, 1)) == 1) {
+      *buffer++ = c;
+      if (c == '\n') {
+        break;
+      }
+    } else if (rc == 0) {
+      if (n == 1) {
+        return 0;
+      } else {
+        break;
+      }
+    } else {
+      if (errno == EINTR) {
+        continue;
+      }
+      return -1;
+    }
+  }
+
+  *buffer = 0;
+  return n;
+}
+
+
+/*  Write a line to a socket  */
+
+ssize_t Writeline_socket(struct img_struct *img, size_t n)
+{
+  size_t      nleft;
+  ssize_t     nwritten;
+  unsigned char *buf1 = img->buf;
+  nleft  = n;
+
+  //printf("socket buf1 %d, %d, %d, %d, %d, %d\n",buf1[0],buf1[1],buf1[2],buf1[3],buf1[4],buf1[5]);
+
+  while (nleft > 0) {
+    if ((nwritten = write(conn_s, buf1, nleft)) <= 0) {
+      if (errno == EINTR) {
+        nwritten = 0;
+      } else {
+        return -1;
+      }
+    }
+    nleft  -= nwritten;
+    buf1 += nwritten;
+  }
+
+  return n;
+
+}
+
+
+
+

sw/airborne/modules/computer_vision/lib/tcp/socket.h

@@ -0,0 +1,17 @@
+#ifndef SOCKET_H
+#define SOCKET_H
+
+#include <unistd.h>             /*  for ssize_t data type  */
+#include "./video/video.h"
+
+#define LISTENQ        (1024)   /*  Backlog for listen()   */
+
+
+/*  Function declarations  */
+int initSocket(void) ;
+ssize_t Readline(void *vptr, size_t maxlen);
+ssize_t Writeline(struct img_struct *img, size_t maxlen);
+int closeSocket(void);
+
+#endif  /*  SOCKET_H  */
+

sw/airborne/modules/computer_vision/lib/udp/socket.c

@@ -22,8 +22,7 @@
 # define SOCKET_ERROR -1
 # define IO_SOCKET ioctl
 
-struct UdpSocket *udp_socket(const char *str_ip_out, const int port_out, const int port_in, const int broadcast)
-{
+struct UdpSocket *udp_socket(const char *str_ip_out, const int port_out, const int port_in, const int broadcast) {
 
   struct UdpSocket *me = malloc(sizeof(struct UdpSocket));
 

sw/airborne/modules/computer_vision/lib/v4l/video.c

@@ -43,10 +43,10 @@
 
 
 pthread_t video_thread;
-void *video_thread_main(void *data);
-void *video_thread_main(void *data)
+void *video_thread_main(void* data);
+void *video_thread_main(void* data)
 {
-  struct vid_struct *vid = (struct vid_struct *)data;
+  struct vid_struct* vid = (struct vid_struct*)data;
   printf("video_thread_main started\n");
   while (1) {
     fd_set fds;
@@ -61,7 +61,7 @@ void *video_thread_main(void *data)
     r = select(vid->fd + 1, &fds, NULL, NULL, &tv);
 
     if (-1 == r) {
-      if (EINTR == errno) { continue; }
+      if (EINTR == errno) continue;
       printf("select err\n");
     }
 
@@ -84,13 +84,13 @@ void *video_thread_main(void *data)
 
     vid->seq++;
 
-    if (vid->trigger) {
+    if(vid->trigger) {
 
       // todo add timestamp again
       //vid->img->timestamp = util_timestamp();
       vid->img->seq = vid->seq;
-      memcpy(vid->img->buf, vid->buffers[buf.index].buf, vid->w * vid->h * 2);
-      vid->trigger = 0;
+      memcpy(vid->img->buf, vid->buffers[buf.index].buf, vid->w*vid->h*2);
+      vid->trigger=0;
     }
 
     if (ioctl(vid->fd, VIDIOC_QBUF, &buf) < 0) {
@@ -108,9 +108,9 @@ int video_init(struct vid_struct *vid)
   unsigned int i;
   enum v4l2_buf_type type;
 
-  vid->seq = 0;
-  vid->trigger = 0;
-  if (vid->n_buffers == 0) { vid->n_buffers = 4; }
+  vid->seq=0;
+  vid->trigger=0;
+  if(vid->n_buffers==0) vid->n_buffers=4;
 
   vid->fd = open(vid->device, O_RDWR | O_NONBLOCK, 0);
 
@@ -148,7 +148,7 @@ int video_init(struct vid_struct *vid)
 
   printf("Buffer count = %d\n", vid->n_buffers);
 
-  vid->buffers = (struct buffer_struct *)calloc(vid->n_buffers, sizeof(struct buffer_struct));
+  vid->buffers = (struct buffer_struct*)calloc(vid->n_buffers, sizeof(struct buffer_struct));
 
   for (i = 0; i < vid->n_buffers; ++i) {
     struct v4l2_buffer buf;
@@ -164,11 +164,11 @@ int video_init(struct vid_struct *vid)
     }
 
     vid->buffers[i].length = buf.length;
-    printf("buffer%d.length=%d\n", i, buf.length);
-    vid->buffers[i].buf = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, vid->fd, buf.m.offset);
+    printf("buffer%d.length=%d\n",i,buf.length);
+    vid->buffers[i].buf = mmap(NULL, buf.length, PROT_READ|PROT_WRITE, MAP_SHARED, vid->fd, buf.m.offset);
 
     if (MAP_FAILED == vid->buffers[i].buf) {
-      printf("mmap() failed.\n");
+      printf ("mmap() failed.\n");
       return -1;
     }
   }
@@ -188,14 +188,14 @@ int video_init(struct vid_struct *vid)
   }
 
   type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-  if (ioctl(vid->fd, VIDIOC_STREAMON, &type) < 0) {
+  if (ioctl(vid->fd, VIDIOC_STREAMON, &type)< 0) {
     printf("ioctl() VIDIOC_STREAMON failed.\n");
     return -1;
   }
 
   //start video thread
   int rc = pthread_create(&video_thread, NULL, video_thread_main, vid);
-  if (rc) {
+  if(rc) {
     printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc);
     return 202;
   }
@@ -207,28 +207,27 @@ void video_close(struct vid_struct *vid)
 {
   int i;
   for (i = 0; i < (int)vid->n_buffers; ++i) {
-    if (-1 == munmap(vid->buffers[i].buf, vid->buffers[i].length)) { printf("munmap() failed.\n"); }
+    if (-1 == munmap(vid->buffers[i].buf, vid->buffers[i].length)) printf("munmap() failed.\n");
   }
   close(vid->fd);
 }
 
 struct img_struct *video_create_image(struct vid_struct *vid)
 {
-  struct img_struct *img = (struct img_struct *)malloc(sizeof(struct img_struct));
-  img->w = vid->w;
-  img->h = vid->h;
-  img->buf = (unsigned char *)malloc(vid->h * vid->w * 2);
+  struct img_struct* img = (struct img_struct*)malloc(sizeof(struct img_struct));
+  img->w=vid->w;
+  img->h=vid->h;
+  img->buf = (unsigned char*)malloc(vid->h*vid->w*2);
   return img;
 }
 
 pthread_mutex_t video_grab_mutex = PTHREAD_MUTEX_INITIALIZER;
 
-void video_grab_image(struct vid_struct *vid, struct img_struct *img)
-{
+void video_grab_image(struct vid_struct *vid, struct img_struct *img) {
   pthread_mutex_lock(&video_grab_mutex);
   vid->img = img;
-  vid->trigger = 1;
+  vid->trigger=1;
   //  while(vid->trigger) pthread_yield();
-  while (vid->trigger) { usleep(1); }
+  while(vid->trigger) usleep(1);
   pthread_mutex_unlock(&video_grab_mutex);
 }

sw/airborne/modules/computer_vision/lib/v4l/video.h

@@ -25,7 +25,7 @@
 #include "../../cv/image.h"
 
 struct buffer_struct {
-  void *buf;
+  void * buf;
   size_t length;
 };
 
@@ -39,7 +39,7 @@ struct vid_struct {
   //private members
   int trigger;
   struct img_struct *img;
-  struct buffer_struct *buffers;
+  struct buffer_struct * buffers;
   int fd;
 };