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_ */