Skip to content

Commit

Permalink
[opticalflow] bebop2 with dual optical flow (#3136)
Browse files Browse the repository at this point in the history 
* Squash on master
* fix bebeop2_flow
* Divergence is more noisy than previous time... added a low-pass filter, and allow for a higher threshold...
* fixed the sim, removed warnings, removed double airframe
* cleanup airframe, remove all warnings, allow placing camera parameters in sections
* PPRZLINK
* Silent warnings and compile errors
* reintegrated pprzlink
---------
Co-authored-by: guidoAI <guido.de.croon@gmail.com>
  • Loading branch information
@dewagter
dewagter committed Oct 9, 2023
1 parent cb97031 commit 4df1320
Show file tree
Hide file tree
Showing 19 changed files with 234 additions and 508 deletions.
208 changes: 83 additions & 125 deletions conf/airframes/examples/bebop2_opticflow.xml
View file
Original file line number Diff line number Diff line change
@@ -1,89 +1,60 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">

<airframe name="bebop2_opticflow">
<description>
BEBOP2 airframe:
- requires fixed dampers
- uses bottom camera for hovering (no GPS and no Optitrack)
- uses the flow from the front camera to navigate
</description>

<firmware name="rotorcraft">

<target name="ap" board="bebop">
<define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000/log"/> <!-- we did not include log before -->
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000/log"/>
<define name="TEXTONS_DICTIONARY_PATH" value="/data/ftp/internal_000"/>
</target>
<!-- <target name="nps" board="pc">

<target name="nps" board="pc">
<module name="fdm" type="gazebo"/>
<define name="VIDEO_CAPTURE_PATH" value="/home/guido/paparazzi/images/"/>
<define name="FILE_LOGGER_PATH" value="/home/guido/paparazzi/log/"/>
<define name="VIDEO_CAPTURE_PATH" value="~/paparazzi/images/"/>
<define name="FILE_LOGGER_PATH" value="~/paparazzi/log/"/>
<define name="TEXTONS_DICTIONARY_PATH" value="~/paparazzi/images/"/>
</target>
-->

<define name="USE_SONAR"/>

<module name="telemetry" type="transparent_udp"/>
<module name="radio_control" type="datalink"/>
<module name="motor_mixing"/>
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="datalink"/>
<module name="gps" type="datalink"/><!-- For logging purposes -->
<module name="stabilization" type="indi_simple"/>

<!--
<module name="ins" type="flow"/>

<!-- Alternative:
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="FALSE"/>
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
<define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
</module>
<module name="ins" type="extended"/>
-->
-->

<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
<define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
<define name="AHRS_FLOATING_HEADING" value="1"/>

<module name="ins" type="flow"/>

<!-- Disable use of GPS: -->
<!-- <define name="INS_INT_GPS_ID" value="ABI_DISABLE"/>
</module> -->

<!-- <module name="geo_mag"/>
<module name="air_data"/>
<module name="gps" type="ubx_ucenter"/> -->

<module name="bebop_cam">
<!-- IMPORTANT to limit these or FPS drops significantly -->
<!-- <define name="MT9F002_TARGET_FPS" value="30"/> --> <!-- Front cam -->
<define name="MT9V117_TARGET_FPS" value="90"/> <!-- Bottom cam -->
</module>
<module name="bebop_cam"/>
<module name="pose_history"/>

<module name="logger_file">
<define name="LOGGER_FILE_PATH" value="/data/ftp/internal_000"/>
<!-- <define name="LOGGER_FILE_PATH" value="/home/guido/paparazzi/log"/> -->
</module>

<module name="logger_file"/>

<module name="cv_opticflow">
<define name="OPTICFLOW_CAMERA" value="bottom_camera"/>

<define name="MAX_HORIZON" value="10"/>
<define name="OPTICFLOW_FX" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
<define name="OPTICFLOW_FY" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
<define name="OPTICFLOW_FOV_W" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
<define name="OPTICFLOW_FOV_H" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->

<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X" value="0.8"/> <!--Obtained from a linefit-->
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y" value="0.85"/> <!--Obtained from a linefit-->
<configure name="OPTICFLOW_MAX_TRACK_CORNERS" value="20"/>
<define name="OPTICFLOW_FEATURE_MANAGEMENT" value="0"/>
<define name="OPTICFLOW_FPS" value="0"/>
<define name="OPTICFLOW_PYRAMID_LEVEL" value="0"/>
<define name="OPTICFLOW_SHOW_FLOW" value="1"/>

<!--
<define name="OPTICFLOW_CAMERA2" value="front_camera"/>
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X_CAMERA2" value="0.8"/>
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y_CAMERA2" value="0.85"/>
<configure name="OPTICFLOW_MAX_TRACK_CORNERS_CAMERA2" value="20"/>
<define name="OPTICFLOW_FEATURE_MANAGEMENT_CAMERA2" value="0"/>
<define name="OPTICFLOW_FPS_CAMERA2" value="0"/>
<define name="OPTICFLOW_PYRAMID_LEVEL_CAMERA2" value="0"/>
<define name="OPTICFLOW_SHOW_FLOW_CAMERA2" value="1"/> -->
</module>

<module name="video_rtp_stream">
Expand All @@ -94,31 +65,12 @@
<define name="VIEWVIDEO_QUALITY_FACTOR" value="40"/>
</module>


<module name="video_capture">
<define name="VIDEO_CAPTURE_CAMERA" value="bottom_camera"/>
</module>

<!-- TODO: perhaps grey because of placement after optical flow? -->
<module name="cv_textons.xml">
<define name="TEXTONS_FPS" value="1"/>
<define name="TEXTONS_CAMERA" value="bottom_camera"/>
<define name="TEXTONS_N_TEXTONS" value="20"/>
<define name="TEXTONS_DICTIONARY_PATH" value="/data/ftp/internal_000"/>
</module>


<module name="optical_flow_landing.xml">
<define name="OFL_COV_SETPOINT" value="-0.075"/>
<define name="OFL_PGAIN" value="0.15"/>
<define name="OFL_IGAIN" value="0.005"/>
<define name="OFL_LP_CONST" value="0.033"/>
<define name="OFL_CLOSE_TO_EDGE" value="0.025"/>
<define name="OFL_IGAIN_ADAPTIVE" value="0.10"/>
<define name="OFL_CONTROL_METHOD" value="0"/> <!-- 1 is adaptive gain -->
<define name="OFL_P_LAND_THRESHOLD" value="0.25"/>
<define name="OFL_COV_LANDING_LIMIT" value="10.0"/>
</module>
<module name="cv_textons"/>
<module name="optical_flow_landing"/>

</firmware>

Expand Down Expand Up @@ -153,16 +105,66 @@
</command_laws>


<section name="BEBOP_BOTTOM_CAMERA" prefix="MT9V117_">
<!-- IMPORTANT to limit these or FPS drops significantly -->
<define name="TARGET_FPS" value="90"/>
</section>

<section name="BEBOP_FRONT_CAMERA" prefix="MT9F002_">
<!-- IMPORTANT to limit these or FPS drops significantly -->
<define name="TARGET_FPS" value="30" />
<define name="OUTPUT_HEIGHT" value="240" />
<define name="OUTPUT_WIDTH" value="240" />
<define name="OFFSET_X" value="0.14" />
<define name="ZOOM" value="3.2"/>
<!--define name="GAIN_GREEN1" value="10.0"/>
<define name="GAIN_GREEN2" value="10.0"/>
<define name="GAIN_BLUE" value="12.5"/>
<define name="GAIN_RED" value="9.0"/-->
<define name="TARGET_EXPOSURE" value="10" />
<define name="GAIN_RED" value="9.0"/>
<define name="TARGET_EXPOSURE" value="10" /-->
</section>

<section name="OPTICAL_FLOW" prefix="OPTICFLOW_">
<define name="CAMERA" value="bottom_camera"/>

<define name="FX" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
<define name="FY" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
<define name="FOV_W" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
<define name="FOV_H" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->

<define name="DEROTATION_CORRECTION_FACTOR_X" value="0.8"/> <!--Obtained from a linefit-->
<define name="DEROTATION_CORRECTION_FACTOR_Y" value="0.85"/> <!--Obtained from a linefit-->
<define name="FEATURE_MANAGEMENT" value="0"/>
<define name="FPS" value="0"/>
<define name="PYRAMID_LEVEL" value="0"/>
<define name="SHOW_FLOW" value="1"/>


<define name="CAMERA2" value="front_camera"/>
<define name="DEROTATION_CORRECTION_FACTOR_X_CAMERA2" value="0.8"/>
<define name="DEROTATION_CORRECTION_FACTOR_Y_CAMERA2" value="0.85"/>
<define name="FEATURE_MANAGEMENT_CAMERA2" value="0"/>
<define name="FPS_CAMERA2" value="0"/>
<define name="PYRAMID_LEVEL_CAMERA2" value="0"/>
<define name="SHOW_FLOW_CAMERA2" value="1"/>
</section>

<section name="OPTICAL_FLOW_LANDING" PREFIX="OFL_">
<define name="COV_SETPOINT" value="-0.075"/>
<define name="PGAIN" value="0.15"/>
<define name="IGAIN" value="0.005"/>
<define name="LP_CONST" value="0.033"/>
<define name="CLOSE_TO_EDGE" value="0.025"/>
<define name="IGAIN_ADAPTIVE" value="0.10"/>
<define name="CONTROL_METHOD" value="0"/> <!-- 1 is adaptive gain -->
<define name="P_LAND_THRESHOLD" value="0.25"/>
<define name="COV_LANDING_LIMIT" value="10.0"/>
</section>

<section name="TEXTONS" prefix="TEXTONS_">
<define name="FPS" value="1"/>
<define name="CAMERA" value="bottom_camera"/>
<define name="N_TEXTONS" value="20"/>
</section>

<section name="AIR_DATA" prefix="AIR_DATA_">
Expand Down Expand Up @@ -222,10 +224,12 @@

<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<!--define name="JSBSIM_MODEL" value="bebop" type="string"/-->
<define name="JSBSIM_MODEL" value="bebop" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<!-- Simulator resolution -->
<define name="MT9F002_SENSOR_RES_DIVIDER" value="4"/>
</section>

<include href="conf/simulator/gazebo/airframes/bebop.xml"/>

<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
Expand Down Expand Up @@ -253,8 +257,6 @@
<define name="G1_R" value="0.0075"/>
<define name="G2_R" value="0.236"/>



<!-- Here it is assumed that your removed the damping from your bebop2!
The dampers do not really damp, but cause oscillation. By removing/
fixing them, the bebop2 will fly much better-->
Expand All @@ -270,15 +272,7 @@
<define name="REF_RATE_Q" value="28.0"/>
<define name="REF_RATE_R" value="28.0"/>

<!-- second order filter parameters -->
<!-- old?
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.7"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.7"/>
-->

<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>

<!-- first order actuator dynamics -->
Expand All @@ -297,12 +291,6 @@
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.52"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
<!-- old?
<define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.55"/>
<define name="ADAPT_MIN_HOVER_THROTTLE" value="0.48"/>
<define name="ADAPT_MAX_HOVER_THROTTLE" value="0.62"/>
<define name="ADAPT_NOISE_FACTOR" value="0.8"/>
-->
</section>

<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
Expand All @@ -318,42 +306,11 @@
<define name="DESCEND_VSPEED" value="-0.75"/>
</section>

<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
</section>

<section name="AUTOPILOT">

<!--
<define name="MODE_STARTUP" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_NAV"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
-->

<!--
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO1" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
-->

<!--
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
-->



<define name="MODE_STARTUP" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_MANUAL" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO1" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_DIRECT"/>


<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
</section>

Expand All @@ -368,5 +325,6 @@
<define name="LOW_BAT_LEVEL" value="11.1" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>

</airframe>

0 comments on commit 4df1320

Please sign in to comment.