[modules] separate opticflow calculation and guidance hover loop

paparazzi · Sep 13, 2015 · 41d52d0 · 41d52d0
1 parent dab660f
commit 41d52d0
Show file tree

Hide file tree

Showing 9 changed files with 71 additions and 44 deletions.
diff --git a/conf/airframes/TUDelft/airframes/ardrone2_opticflow.xml b/conf/airframes/TUDelft/airframes/ardrone2_opticflow.xml
@@ -29,6 +29,7 @@
     <load name="send_imu_mag_current.xml"/>
     <load name="logger_file.xml"/>
     <load name="cv_opticflow.xml"/>
+    <load name="opticflow_hover.xml"/>
   </modules>
 
   <commands>

diff --git a/conf/airframes/TUDelft/conf.xml b/conf/airframes/TUDelft/conf.xml
@@ -7,8 +7,8 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int_quat.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/estimation/body_to_imu.xml"
-   settings_modules="modules/gps_ubx_ucenter.xml modules/air_data.xml modules/geo_mag.xml"
    gui_color="red"
+   settings_modules="modules/gps_ubx_ucenter.xml modules/air_data.xml modules/geo_mag.xml"
   />
   <aircraft
    name="ARDrone2_indi"
@@ -29,7 +29,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int_quat.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/estimation/body_to_imu.xml"
-   settings_modules="modules/gps_ubx_ucenter.xml modules/cv_opticflow.xml"
+   settings_modules="modules/gps_ubx_ucenter.xml modules/cv_opticflow.xml modules/opticflow_hover.xml"
    gui_color="red"
   />
   <aircraft
@@ -51,7 +51,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_float_mlkf.xml settings/control/stabilization_att_int_quat.xml"
-   settings_modules="modules/geo_mag.xml modules/air_data.xml modules/bebop_front_camera.xml"
+   settings_modules="modules/geo_mag.xml modules/air_data.xml modules/video_thread.xml modules/video_rtp_stream.xml"
    gui_color="green"
   />
   <aircraft

diff --git a/conf/airframes/examples/ardrone2_opticflow_hover.xml b/conf/airframes/examples/ardrone2_opticflow_hover.xml
@@ -29,6 +29,7 @@
     <load name="send_imu_mag_current.xml"/>
     <load name="logger_file.xml"/>
     <load name="cv_opticflow.xml"/>
+    <load name="opticflow_hover.xml"/>
   </modules>
 
   <commands>

diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml
@@ -359,7 +359,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
-   settings_modules="modules/gps_ubx_ucenter.xml modules/cv_opticflow.xml"
+   settings_modules="modules/gps_ubx_ucenter.xml modules/cv_opticflow.xml modules/opticflow_hover.xml"
    gui_color="blue"
   />
   <aircraft

diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml
@@ -3,23 +3,11 @@
 <module name="cv_opticflow" dir="computer_vision">
   <doc>
     <description>
-        Hovers the drone based on optical flow made for Linux video Devices.
-
-        Computes Pitch- and roll attide from downward looking camera looking at a textured floor.
-        - Sonar is required.
-        - Controller can hold position
+      Compute optical flow.
+      Made for Linux video Devices.
+      Computes x and y velocity using optical flow and distance to ground (using sonar).
     </description>
 
-    <!-- Satbilization parameters and gains -->
-    <section name="VISION" prefix="VISION_">
-      <define name="PHI_PGAIN" value="400" description="Optic flow proportional gain on the roll velocity error"/>
-      <define name="PHI_IGAIN" value="20" description="Optic flow integrated gain on the summed roll velocity error"/>
-      <define name="THETA_PGAIN" value="400" description="Optic flow proportional gain on the pitch velocity error"/>
-      <define name="THETA_IGAIN" value="20" description="Optic flow integrated gain on the summed pitch velocity error"/>
-      <define name="DESIRED_VX" value="0" description="The desired velocity in the body frame x direction"/>
-      <define name="DESIRED_VY" value="0" description="The desired velocity in the body frame y direction"/>
-    </section>
-
     <!-- Optical flow calculation parameters -->
     <section name="OPTICFLOW" prefix="OPTICFLOW_">
       <define name="AGL_ID" value="ABI_SENDER_ID" description="ABI sender id for AGL message (sonar measurement) (default: ABI_BROADCAST)"/>
@@ -51,16 +39,7 @@
   </doc>
 
   <settings>
-	  <dl_settings NAME="Vision stabilization">
-      <!-- Satabilization loop parameters and gains -->
-      <dl_settings name="vision_stab">
-        <dl_setting var="opticflow_stab.phi_pgain" module="computer_vision/opticflow_module" min="0" step="1" max="10000" shortname="kp_v_phi" param="VISION_PHI_PGAIN"/>
-        <dl_setting var="opticflow_stab.phi_igain" module="computer_vision/opticflow_module" min="0" step="1" max="1000" shortname="ki_v_phi" param="VISION_PHI_IGAIN"/>
-        <dl_setting var="opticflow_stab.theta_pgain" module="computer_vision/opticflow_module" min="0" step="1" max="10000" shortname="kp_v_theta" param="VISION_THETA_PGAIN"/>
-        <dl_setting var="opticflow_stab.theta_igain" module="computer_vision/opticflow_module" min="0" step="1" max="1000" shortname="ki_v_theta" param="VISION_THETA_IGAIN"/>
-        <dl_setting var="opticflow_stab.desired_vx" module="computer_vision/opticflow_module" min="-5" step="0.01" max="5" shortname="desired_vx" param="VISION_DESIRED_VX"/>
-        <dl_setting var="opticflow_stab.desired_vy" module="computer_vision/opticflow_module" min="-5" step="0.01" max="5" shortname="desired_vy" param="VISION_DESIRED_VY"/>
-      </dl_settings>
+	<dl_settings NAME="Vision stabilization">
 
       <!-- Optical flow calculations parameters for Lucas Kanade and FAST9 -->
       <dl_settings name="vision_calc">
@@ -92,11 +71,10 @@
     <file name="rtp.c" dir="modules/computer_vision/lib/encoding"/>
     <file name="v4l2.c" dir="modules/computer_vision/lib/v4l"/>
 
-    <!-- The optical flow module (calculator+stabilization) -->
+    <!-- The optical flow module (calculator) -->
     <file name="opticflow_module.c"/>
     <file name="opticflow_calculator.c" dir="modules/computer_vision/opticflow"/>
     <file name="size_divergence.c" dir="modules/computer_vision/opticflow"/>
-    <file name="stabilization_opticflow.c" dir="modules/computer_vision/opticflow"/>
     <file name="linear_flow_fit.c" dir="modules/computer_vision/opticflow"/>
     <file name="pprz_algebra_float.c" dir="math"/>
     <file name="pprz_matrix_decomp_float.c" dir="math"/>
@@ -116,9 +94,6 @@
       else
         ap.CFLAGS += $(VIEWVID_CFLAGS) -DVIEWVIDEO_USE_NC
       endif
-
-      ap.CFLAGS += -DGUIDANCE_V_MODE_MODULE_SETTING=GUIDANCE_V_MODE_HOVER
-      ap.CFLAGS += -DGUIDANCE_H_MODE_MODULE_SETTING=GUIDANCE_H_MODE_MODULE
     </raw>
   </makefile>
 

diff --git a/conf/modules/opticflow_hover.xml b/conf/modules/opticflow_hover.xml
@@ -0,0 +1,52 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="opticflow_hover" dir="guidance_opticflow">
+  <doc>
+    <description>
+        Hovers the drone based on optical flow made for Linux video Devices.
+
+        Computes Pitch- and roll attide from downward looking camera looking at a textured floor.
+        - Sonar is required.
+        - Controller can hold position
+    </description>
+
+    <!-- Satbilization parameters and gains -->
+    <section name="VISION" prefix="VISION_">
+      <define name="PHI_PGAIN" value="400" description="Optic flow proportional gain on the roll velocity error"/>
+      <define name="PHI_IGAIN" value="20" description="Optic flow integrated gain on the summed roll velocity error"/>
+      <define name="THETA_PGAIN" value="400" description="Optic flow proportional gain on the pitch velocity error"/>
+      <define name="THETA_IGAIN" value="20" description="Optic flow integrated gain on the summed pitch velocity error"/>
+      <define name="DESIRED_VX" value="0" description="The desired velocity in the body frame x direction"/>
+      <define name="DESIRED_VY" value="0" description="The desired velocity in the body frame y direction"/>
+    </section>
+  </doc>
+
+  <settings>
+	<dl_settings NAME="Vision stabilization">
+      <!-- Satabilization loop parameters and gains -->
+      <dl_settings name="vision_stab">
+        <dl_setting var="opticflow_stab.phi_pgain" module="computer_vision/opticflow_module" min="0" step="1" max="10000" shortname="kp_v_phi" param="VISION_PHI_PGAIN"/>
+        <dl_setting var="opticflow_stab.phi_igain" module="computer_vision/opticflow_module" min="0" step="1" max="1000" shortname="ki_v_phi" param="VISION_PHI_IGAIN"/>
+        <dl_setting var="opticflow_stab.theta_pgain" module="computer_vision/opticflow_module" min="0" step="1" max="10000" shortname="kp_v_theta" param="VISION_THETA_PGAIN"/>
+        <dl_setting var="opticflow_stab.theta_igain" module="computer_vision/opticflow_module" min="0" step="1" max="1000" shortname="ki_v_theta" param="VISION_THETA_IGAIN"/>
+        <dl_setting var="opticflow_stab.desired_vx" module="computer_vision/opticflow_module" min="-5" step="0.01" max="5" shortname="desired_vx" param="VISION_DESIRED_VX"/>
+        <dl_setting var="opticflow_stab.desired_vy" module="computer_vision/opticflow_module" min="-5" step="0.01" max="5" shortname="desired_vy" param="VISION_DESIRED_VY"/>
+      </dl_settings>
+    </dl_settings>
+  </settings>
+
+  <depends>cv_opticflow</depends>
+
+  <header>
+    <file name="guidance_opticflow_hover.h"/>
+  </header>
+
+  <makefile>
+    <file name="guidance_opticflow_hover.c"/>
+    <raw>
+      ap.CFLAGS += -DGUIDANCE_V_MODE_MODULE_SETTING=GUIDANCE_V_MODE_HOVER
+      ap.CFLAGS += -DGUIDANCE_H_MODE_MODULE_SETTING=GUIDANCE_H_MODE_MODULE
+    </raw>
+  </makefile>
+
+</module>
diff --git a/sw/airborne/modules/computer_vision/opticflow_module.h b/sw/airborne/modules/computer_vision/opticflow_module.h
@@ -20,17 +20,15 @@
 
 /**
  * @file modules/computer_vision/opticflow_module.h
- * @brief optical-flow based hovering for Parrot AR.Drone 2.0
+ * @brief optical-flow calculation for Parrot Drones
  *
- * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
  */
 
 #ifndef OPTICFLOW_MODULE_H
 #define OPTICFLOW_MODULE_H
 
-// Include opticflow calculator and stabilization loops
+// Include opticflow calculator
 #include "opticflow/opticflow_calculator.h"
-#include "opticflow/stabilization_opticflow.h"
 
 // Needed for settings
 extern struct opticflow_t opticflow;

diff --git a/...ision/opticflow/stabilization_opticflow.c → ...ance_opticflow/guidance_opticflow_hover.c b/...ision/opticflow/stabilization_opticflow.c → ...ance_opticflow/guidance_opticflow_hover.c
@@ -20,15 +20,15 @@
  */
 
 /**
- * @file modules/computer_vision/opticflow/stabilization_opticflow.c
+ * @file modules/guidance_opticflow/guidance_opticflow_hover.c
  * @brief Optical-flow based control for Linux based systems
  *
  * Control loops for optic flow based hovering.
  * Computes setpoint for the lower level attitude stabilization to control horizontal velocity.
  */
 
 // Own Header
-#include "stabilization_opticflow.h"
+#include "guidance_opticflow_hover.h"
 
 #include "subsystems/abi.h"
 

diff --git a/...ision/opticflow/stabilization_opticflow.h → ...ance_opticflow/guidance_opticflow_hover.h b/...ision/opticflow/stabilization_opticflow.h → ...ance_opticflow/guidance_opticflow_hover.h
@@ -20,15 +20,15 @@
  */
 
 /**
- * @file modules/computer_vision/opticflow/stabilization_opticflow.h
+ * @file modules/guidance_opticflow/guidance_opticflow_hover.h
  * @brief Optical-flow based control for Linux based systems
  *
  * Control loops for optic flow based hovering.
  * Computes setpoint for the lower level attitude stabilization to control horizontal velocity.
  */
 
-#ifndef CV_STABILIZATION_OPTICFLOW_H_
-#define CV_STABILIZATION_OPTICFLOW_H_
+#ifndef GUIDANCE_OPTICFLOW_HOVER_H_
+#define GUIDANCE_OPTICFLOW_HOVER_H_
 
 #include "std.h"
 #include "math/pprz_algebra_int.h"
@@ -54,4 +54,4 @@ extern void guidance_h_module_enter(void);
 extern void guidance_h_module_read_rc(void);
 extern void guidance_h_module_run(bool_t in_flight);
 
-#endif /* CV_STABILIZATION_OPTICFLOW_H_ */
+#endif /* GUIDANCE_OPTICFLOW_HOVER_H_ */