Merge pull request #1062 from paparazzi/opticflow

Add cv_opticflow.xml module. Used to for hover stabilization on an ARDrone2. Also adds AP_MODE_MODULE to make it easier to add extra "external" control loops.
paparazzi · Feb 5, 2015 · 17b200a · 17b200a
2 parents 9dd169c + 5fd3808
commit 17b200a
Show file tree

Hide file tree

Showing 36 changed files with 9,914 additions and 13 deletions.
diff --git a/conf/airframes/ardrone2_opticflow_hover.xml b/conf/airframes/ardrone2_opticflow_hover.xml
@@ -0,0 +1,230 @@
+<!DOCTYPE airframe SYSTEM "airframe.dtd">
+
+<airframe name="ardrone2_raw">
+
+  <firmware name="rotorcraft">
+    <target name="ap" board="ardrone2_raw">
+      <subsystem name="telemetry" type="transparent_udp"/>
+      <subsystem name="radio_control" type="datalink"/>
+    </target>
+
+    <!--target name="nps" board="pc">
+      <subsystem name="fdm" type="jsbsim"/>
+      <subsystem name="radio_control" type="ppm"/>
+    </target-->
+
+    <define name="USE_SONAR" value="TRUE"/>
+
+	<!-- Subsystem section -->
+    <subsystem name="motor_mixing"/>
+    <subsystem name="actuators" type="ardrone2"/>
+    <subsystem name="imu" type="ardrone2"/>
+    <subsystem name="gps" type="ublox"/>
+    <subsystem name="stabilization" type="int_quat"/>
+    <subsystem name="ahrs" type="int_cmpl_quat"/>
+    <subsystem name="ins" type="extended"/>
+  </firmware>
+
+  <modules main_freq="512">
+    <load name="gps_ubx_ucenter.xml"/>
+    <load name="send_imu_mag_current.xml"/>
+    <load name="file_logger.xml"/>
+    <load name="cv_opticflow.xml"/>
+  </modules>
+
+  <commands>
+    <axis name="PITCH" failsafe_value="0"/>
+    <axis name="ROLL" failsafe_value="0"/>
+    <axis name="YAW" failsafe_value="0"/>
+    <axis name="THRUST" failsafe_value="3000"/>
+  </commands>
+
+  <servos driver="Default">
+    <servo name="TOP_LEFT" no="0" min="0" neutral="1" max="500"/>
+    <servo name="TOP_RIGHT" no="1" min="0" neutral="1" max="500"/>
+    <servo name="BOTTOM_RIGHT" no="2" min="0" neutral="1" max="500"/>
+    <servo name="BOTTOM_LEFT" no="3" min="0" neutral="1" max="500"/>
+  </servos>
+
+  <section name="MIXING" prefix="MOTOR_MIXING_">
+    <define name="TRIM_ROLL" value="0"/>
+    <define name="TRIM_PITCH" value="0"/>
+    <define name="TRIM_YAW" value="0"/>
+    <define name="NB_MOTOR" value="4"/>
+    <define name="SCALE" value="255"/>
+
+    <!-- Time cross layout (X), with order NW (CW), NE (CCW), SE (CW), SW (CCW) -->
+    <define name="ROLL_COEF" value="{   256, -256, -256,  256 }"/>
+    <define name="PITCH_COEF" value="{  256,  256, -256, -256 }"/>
+    <define name="YAW_COEF" value="{   -256,  256, -256,  256 }"/>
+    <define name="THRUST_COEF" value="{ 256,  256,  256,  256 }"/>
+  </section>
+
+  <command_laws>
+    <call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
+    <set servo="TOP_LEFT" value="motor_mixing.commands[0]"/>
+    <set servo="TOP_RIGHT" value="motor_mixing.commands[1]"/>
+    <set servo="BOTTOM_RIGHT" value="motor_mixing.commands[2]"/>
+    <set servo="BOTTOM_LEFT" value="motor_mixing.commands[3]"/>
+  </command_laws>
+
+   <section name="VISION" prefix="VISION_">
+    <define name="HOVER" value="FALSE"/>
+    <define name="PHI_PGAIN" value="500"/>
+    <define name="PHI_IGAIN" value="10"/>
+    <define name="THETA_PGAIN" value="500"/>
+    <define name="THETA_IGAIN" value="10"/>
+    <define name="DESIRED_VX" value="0"/>
+    <define name="DESIRED_VY" value="0"/>
+   </section>
+
+  <section name="IMU" prefix="IMU_">
+    <!-- Accelero -->
+    <define name="ACCEL_X_NEUTRAL" value="2048"/>
+    <define name="ACCEL_Y_NEUTRAL" value="2048"/>
+    <define name="ACCEL_Z_NEUTRAL" value="2048"/>
+
+    <!-- Magneto calibration -->
+    <define name="MAG_X_NEUTRAL" value="0"/>
+    <define name="MAG_Y_NEUTRAL" value="0"/>
+    <define name="MAG_Z_NEUTRAL" value="-180"/>
+    <define name="MAG_X_SENS" value="16." integer="16"/>
+    <define name="MAG_Y_SENS" value="16." integer="16"/>
+    <define name="MAG_Z_SENS" value="16." integer="16"/>
+
+    <!-- Magneto current calibration -->
+    <define name="MAG_X_CURRENT_COEF" value="0.0"/>
+    <define name="MAG_Y_CURRENT_COEF" value="0.0"/>
+    <define name="MAG_Z_CURRENT_COEF" value="0.0"/>
+
+    <define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
+  </section>
+
+  <!-- local magnetic field -->
+  <!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
+  <section name="AHRS" prefix="AHRS_">
+    <!-- Toulouse -->
+    <define name="H_X" value="0.513081"/>
+    <define name="H_Y" value="-0.00242783"/>
+    <define name="H_Z" value="0.858336"/>
+    <!-- Delft -->
+    <!--define name="H_X" value="0.3892503"/>
+    <define name="H_Y" value="0.0017972"/>
+    <define name="H_Z" value="0.9211303"/ -->
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="SONAR_MAX_RANGE" value="2.2"/>
+    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
+  </section>
+
+  <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
+    <!-- setpoints -->
+    <define name="SP_MAX_P" value="10000"/>
+    <define name="SP_MAX_Q" value="10000"/>
+    <define name="SP_MAX_R" value="10000"/>
+    <define name="DEADBAND_P" value="20"/>
+    <define name="DEADBAND_Q" value="20"/>
+    <define name="DEADBAND_R" value="200"/>
+    <define name="REF_TAU" value="4"/>
+
+    <!-- feedback -->
+    <define name="GAIN_P" value="400"/>
+    <define name="GAIN_Q" value="400"/>
+    <define name="GAIN_R" value="350"/>
+
+    <define name="IGAIN_P" value="75"/>
+    <define name="IGAIN_Q" value="75"/>
+    <define name="IGAIN_R" value="50"/>
+
+    <!-- feedforward -->
+    <define name="DDGAIN_P" value="300"/>
+    <define name="DDGAIN_Q" value="300"/>
+    <define name="DDGAIN_R" value="300"/>
+  </section>
+
+  <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
+    <!-- setpoints -->
+    <define name="SP_MAX_PHI" value="45" unit="deg"/>
+    <define name="SP_MAX_THETA" value="45" unit="deg"/>
+    <define name="SP_MAX_R" value="600" unit="deg/s"/>
+    <define name="DEADBAND_A" value="0"/>
+    <define name="DEADBAND_E" value="0"/>
+    <define name="DEADBAND_R" value="250"/>
+
+    <!-- reference -->
+    <define name="REF_OMEGA_P" value="450" unit="deg/s"/>
+    <define name="REF_ZETA_P" value="0.9"/>
+    <define name="REF_MAX_P" value="600." unit="deg/s"/>
+    <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
+
+    <define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
+    <define name="REF_ZETA_Q" value="0.9"/>
+    <define name="REF_MAX_Q" value="600." unit="deg/s"/>
+    <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
+
+    <define name="REF_OMEGA_R" value="200" unit="deg/s"/>
+    <define name="REF_ZETA_R" value="0.9"/>
+    <define name="REF_MAX_R" value="300." unit="deg/s"/>
+    <define name="REF_MAX_RDOT" value="RadOfDeg(4000.)"/>
+
+    <!-- feedback -->
+    <define name="PHI_PGAIN" value="592"/>
+    <define name="PHI_DGAIN" value="303"/>
+    <define name="PHI_IGAIN" value="0"/>
+
+    <define name="THETA_PGAIN" value="606"/>
+    <define name="THETA_DGAIN" value="303"/>
+    <define name="THETA_IGAIN" value="0"/>
+
+    <define name="PSI_PGAIN" value="529"/>
+    <define name="PSI_DGAIN" value="353"/>
+    <define name="PSI_IGAIN" value="0"/>
+
+    <!-- feedforward -->
+    <define name="PHI_DDGAIN" value="0"/>
+    <define name="THETA_DDGAIN" value="0"/>
+    <define name="PSI_DDGAIN" value="52"/>
+  </section>
+
+  <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
+    <define name="HOVER_KP" value="283"/>
+    <define name="HOVER_KD" value="82"/>
+    <define name="HOVER_KI" value="13"/>
+    <define name="NOMINAL_HOVER_THROTTLE" value="0.655"/>
+    <define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
+  </section>
+
+  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
+    <!-- Good weather -->
+    <define name="MAX_BANK" value="20" unit="deg"/>
+    <!-- Bad weather -->
+    <!-- define name="MAX_BANK" value="32" unit="deg"/ -->
+    <define name="PGAIN" value="79"/>
+    <define name="DGAIN" value="100"/>
+    <define name="IGAIN" value="30"/>
+  </section>
+
+  <section name="SIMULATOR" prefix="NPS_">
+    <define name="ACTUATOR_NAMES" value="{&quot;nw_motor&quot;, &quot;ne_motor&quot;, &quot;se_motor&quot;, &quot;sw_motor&quot;}"/>
+    <define name="JSBSIM_MODEL" value="&quot;simple_ardrone2&quot;"/>
+    <define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_ardrone2.h&quot;"/>
+  </section>
+
+  <section name="AUTOPILOT">
+    <define name="MODE_STARTUP" value="AP_MODE_NAV"/>
+    <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
+    <define name="MODE_AUTO1" value="AP_MODE_MODULE"/>
+    <define name="MODE_AUTO2" value="AP_MODE_NAV"/>
+  </section>
+
+  <section name="BAT">
+    <define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
+    <define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
+    <define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
+  </section>
+</airframe>
diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml
@@ -318,6 +318,17 @@
    settings_modules="modules/meteo_france_DAQ.xml"
    gui_color="blue"
   />
+  <aircraft
+   name="ardrone2_opticflow"
+   ac_id="2"
+   airframe="airframes/ardrone2_opticflow_hover.xml"
+   radio="radios/cockpitSX.xml"
+   telemetry="telemetry/default_ardrone.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"
+   gui_color="blue"
+  />
   <aircraft
    name="ardrone2_raw"
    ac_id="201"

diff --git a/conf/messages.xml b/conf/messages.xml
@@ -1972,8 +1972,30 @@
     <field name="t" type="uint32"/>
   </message>
 
- <!--228 is free -->
- <!--229 is free -->
+  <message name="OF_HOVER" id="228">
+    <field name="FPS"        type="float"/>
+    <field name="dx"         type="float"/>
+    <field name="dy"         type="float"/>
+    <field name="dx_trans"   type="float"/>
+    <field name="dy_trans"   type="float"/>
+    <field name="diff_roll"  type="float"/>
+    <field name="diff_pitch" type="float"/>
+    <field name="velx"       type="float"/>
+    <field name="vely"       type="float"/>
+    <field name="velx_Ned"   type="float"/>
+    <field name="vely_Ned"   type="float"/>
+    <field name="z_sonar"    type="float"/>
+    <field name="count"      type="int32"/>
+ </message>
+
+  <message name="VISION_STABILIZATION" id="229">
+    <field name="velx"      type="float"/>
+    <field name="vely"      type="float"/>
+    <field name="velx_i"    type="float"/>
+    <field name="vely_i"    type="float"/>
+    <field name="cmd_phi"   type="int32"/>
+    <field name="cmd_theta" type="int32"/>
+  </message>
 
   <message name="AHRS_ARDRONE2" id="230">
     <field name="state" type="uint32" />
@@ -1997,7 +2019,7 @@
     <field name="rc_status"    type="uint8" values="OK|LOST|REALLY_LOST"/>
     <field name="frame_rate"   type="uint8" unit="Hz"/>
     <field name="gps_status"   type="uint8" values="NO_FIX|NA|NA|3Dfix"/>
-    <field name="ap_mode"      type="uint8" values="KILL|FAILSAFE|HOME|RATE_DIRECT|ATTITUDE_DIRECT|RATE_RC_CLIMB|ATTITUDE_RC_CLIMB|ATTITUDE_CLIMB|RATE_Z_HOLD|ATTITUDE_Z_HOLD|HOVER_DIRECT|HOVER_CLIMB|HOVER_Z_HOLD|NAV|RC_DIRECT|CARE_FREE|FORWARD"/>
+    <field name="ap_mode"      type="uint8" values="KILL|FAILSAFE|HOME|RATE_DIRECT|ATTITUDE_DIRECT|RATE_RC_CLIMB|ATTITUDE_RC_CLIMB|ATTITUDE_CLIMB|RATE_Z_HOLD|ATTITUDE_Z_HOLD|HOVER_DIRECT|HOVER_CLIMB|HOVER_Z_HOLD|NAV|RC_DIRECT|CARE_FREE|FORWARD|MODULE"/>
     <field name="ap_in_flight" type="uint8" values="ON_GROUND|IN_FLIGHT"/>
     <field name="ap_motors_on" type="uint8" values="MOTORS_OFF|MOTORS_ON"/>
     <field name="ap_h_mode"    type="uint8" values="KILL|RATE|ATTITUDE|HOVER|NAV|CF"/>

diff --git a/conf/modules/ctrl_module_demo.xml b/conf/modules/ctrl_module_demo.xml
@@ -0,0 +1,30 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="ctrl_module_demo" dir="ctrl">
+  <doc>
+    <description>
+        Demo Control Module
+
+        Rate-controller as module sample
+    </description>
+  </doc>
+  <settings>
+    <dl_settings>
+    <dl_settings NAME="CtrlModDemo">
+      <dl_setting var="ctrl_module_demo_pr_ff_gain" min="0" step="0.01" max="1" module="ctrl/ctrl_module_demo" shortname="pr_ff"/>
+      <dl_setting var="ctrl_module_demo_pr_d_gain"  min="0" step="0.01" max="1" module="ctrl/ctrl_module_demo" shortname="pr_d"/>
+      <dl_setting var="ctrl_module_demo_y_ff_gain"  min="0" step="0.01" max="1" module="ctrl/ctrl_module_demo" shortname="y_ff"/>
+      <dl_setting var="ctrl_module_demo_y_d_gain"   min="0" step="0.01" max="1" module="ctrl/ctrl_module_demo" shortname="y_d"/>
+    </dl_settings>    </dl_settings>
+  </settings>
+
+  <header>
+    <file name="ctrl_module_demo.h"/>
+  </header>
+
+  <makefile>
+    <file name="ctrl_module_demo.c"/>
+  </makefile>
+
+</module>
+
diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml
@@ -0,0 +1,75 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="cv_opticflow" dir="computer_vision">
+  <doc>
+    <description>
+        Compute Optic Flow from Ardrone2 Bottom Camera
+
+        Computes Pitch- and rollrate corrected optic flow from downward looking
+        ARDrone2 camera looking at a textured floor.
+
+        - Sonar is required.
+        - Controller can hold position
+    </description>
+
+    <section name="VISION" prefix="VISION_">
+      <define name="HOVER" value="FALSE" description="TRUE/FALSE active or not"/>
+      <define name="PHI_PGAIN" value="500" description="optic flow pgain"/>
+      <define name="PHI_IGAIN" value="10" description="optic flow igain"/>
+      <define name="THETA_PGAIN" value="500" description="optic flow pgain"/>
+      <define name="THETA_IGAIN" value="10" description="optic flow igain"/>
+      <define name="DESIRED_VX" value="0" description="feedforward optic flow vx"/>
+      <define name="DESIRED_VY" value="0" description="feedforward optic flow vy"/>
+    </section>
+    <define name="DOWNLINK_VIDEO" value="FALSE" description="Also stream video: warning: this makes the optic flow slow: DEBUGGING only" />
+    <define name="OPTICFLOW_AGL_ID" value="ABI_SENDER_ID" description="ABI sender id for AGL message (sonar measurement) (default: ABI_BROADCAST)"/>
+  </doc>
+  <settings>
+    <dl_settings>
+	<dl_settings NAME="Vision Loop">
+        <dl_setting var="activate_opticflow_hover" min="0" step="1" max="1" module="computer_vision/opticflow/hover_stabilization" shortname="hover" param="VISION_HOVER" values="FALSE|TRUE"/>
+        <dl_setting var="vision_phi_pgain" min="0" step="1" max="10000" shortname="kp_v_phi" param="VISION_PHI_PGAIN"/>
+        <dl_setting var="vision_phi_igain" min="0" step="1" max="1000" shortname="ki_v_phi" param="VISION_PHI_IGAIN"/>
+        <dl_setting var="vision_theta_pgain" min="0" step="1" max="10000" shortname="kp_v_theta" param="VISION_THETA_PGAIN"/>
+        <dl_setting var="vision_theta_igain" min="0" step="1" max="1000" shortname="ki_v_theta" param="VISION_THETA_IGAIN"/>
+        <dl_setting var="vision_desired_vx" min="-5" step="0.01" max="5" shortname="desired_vx" param="VISION_DESIRED_VX"/>
+        <dl_setting var="vision_desired_vy" min="-5" step="0.01" max="5" shortname="desired_vy" param="VISION_DESIRED_VY"/>
+    </dl_settings>    </dl_settings>
+  </settings>
+
+  <header>
+    <file name="opticflow_module.h"/>
+  </header>
+
+  <init fun="opticflow_module_init()"/>
+
+  <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"/>
+    <file name="video.c" dir="modules/computer_vision/lib/v4l"/>
+    <define name="modules/computer_vision/cv" type="include"/>
+    <define name="modules/computer_vision/lib" type="include"/>
+    <define name="pthread" type="raw"/>
+    <define name="__USE_GNU"/>
+    <flag name="LDFLAGS" value="pthread"/>
+    <flag name="LDFLAGS" value="lrt"/>
+    <flag name="LDFLAGS" value="static"/>
+  </makefile>
+  <makefile target="nps">
+    <file name="viewvideo_nps.c"/>
+  </makefile>
+
+</module>
+