Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Additional module of_avoidance #90

Open
wants to merge 3 commits into
base: mavlabCourse2023
Choose a base branch
from
Open

Additional module of_avoidance #90

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1ff994b
Added the module with the xml files necessary to build it and the up…
Mar 31, 2023
386b049
Removed redudant folder
Apr 4, 2023
7a65ac4
Merge branch 'tudelft:mavlabCourse2023' into team2_2023
christiaantheunisse Apr 4, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions conf/abi.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -246,6 +246,14 @@
<field name="vel_sp" type="struct FloatVect3 *" unit="m/s"/>
</message>

<message name="DIVERGENCE_SAFE_HEADING" id="37">
<field name="i_safe" type="uint8_t" unit="us"/>
</message>

<message name="PAUSE_THREAD" id="38">
<field name="pause_dura" type="uint8_t" unit="s"/>
</message>

</msg_class>

</protocol>
301 changes: 301 additions & 0 deletions conf/airframes/tudelft/bebop_of_avoidance.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,301 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">

<airframe name="bebop_avoider">
<description>
This file runs the `of_avoidance` module made by team 2 from 2023 for the AFMAV course on the Bebop drone.
</description>


<firmware name="rotorcraft">
<target name="ap" board="bebop">
<define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
<define name="LOGGER_FILE_PATH" value="/data/ftp/internal_000/log"/>
<define name="MT9V117_TARGET_FPS" value="20"/>

<!-- Detect orange -->
<define name="COLOR_OBJECT_DETECTOR_LUM_MIN1" value="30"/>
<define name="COLOR_OBJECT_DETECTOR_LUM_MAX1" value="190"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MIN1" value="70"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MAX1" value="130"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MIN1" value="150"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MAX1" value="190"/>
</target>
<target name="nps" board="pc">
<module name="fdm" type="gazebo"/>
<define name="VIDEO_CAPTURE_PATH" value="/tmp/paparazzi/images"/>
<define name="LOGGER_FILE_PATH" value="/tmp/paparazzi/log"/>

<!-- Detect orange -->
<define name="COLOR_OBJECT_DETECTOR_LUM_MIN1" value="41"/>
<define name="COLOR_OBJECT_DETECTOR_LUM_MAX1" value="183"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MIN1" value="53"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MAX1" value="121"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MIN1" value="134"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MAX1" value="249"/>
</target>

<define name="ARRIVED_AT_WAYPOINT" value="0.5"/><!-- Detect arrival at waypoint when within 0.5m -->

<!-- Subsystem section -->
<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="stabilization" type="indi_simple"/>
<module name="stabilization" type="rate_indi"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="FALSE"/>
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
</module>
<module name="ins" type="extended"/>

<module name="logger_file"/>

<!-- Video/Camera modules -->
<module name="bebop_cam"/>
<!--module name="bebop_ae_awb">
<define name="CV_AE_AWB_VERBOSE" value="0" />
</module-->
<module name="video_capture">
<define name="VIDEO_CAPTURE_CAMERA" value="front_camera"/>
<define name="VIDEO_CAPTURE_FPS" value="10"/>
</module>

<!-- Our pilot module -->
<module name="object_avoider">
<define name="OFF_DIV_SAFE_INDEX" value="37"/>
<!-- Autopilot settings -->
<define name="N_DIRBLOCKS" value="5" description="Possible heading directions"/>
<define name="FOV_ANGLE" value="160" description="Determines the size of the steering correction when detecting an obstacle"/>
<define name="DIR_CHANGE_THRESHOLD" value="6" description="Number of consecutive measurements necessary to decide to steer away"/>
<define name="PAUSE_TIME" value="20" description="The confidence values reset and aren't updated for (PAUSE_TIME * 100ms) after a steer correction"/>
<define name="PAUSE_V_FACTOR" value="0.4" description="The percentage of the default speed it has when pausing [0.0 - 1.0]"/>
</module>

<module name="video_rtp_stream">
<define name="VIEWVIDEO_CAMERA" value="front_camera"/>
<define name="VIEWVIDEO_CAMERA2" value="bottom_camera"/>
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
<define name="VIEWVIDEO_QUALITY_FACTOR" value="40"/>
</module>

<!-- Our image processing module -->
<module name="opencv_optical_flow">
<define name="OPENCVDEMO_CAMERA" value="front_camera"/>
<define name="OPENCVDEMO_FPS" value="0" />

<!-- CROP window settings -->
<define name="RESIZE" value="0.25" description="Resize factor use for the camera image in x and y direction"/>
<define name="CROP_X" value="10" description="Crop offset in x-direction before resizing [0, 520]"/>
<define name="CROP_Y" value="50" description="Crop offset in y-direction before resizing [0, 240]"/>
<!-- CROP_WIDTH is not necessary since it is always symmetric around the center. Therefore CROP_X contains all the necessary info. -->
<define name="CROP_HEIGHT" value="50" description="Crop size in y-direction before resizing [0, 240]"/>

<!-- OF settings -->
<define name="OFF_PYR_SCALE" value="0.5" description="Farneback OF setting"/>
<define name="OFF_LEVELS" value="1" description="Farneback OF setting"/>
<define name="OFF_WINSIZE" value="15" description="Farneback OF setting"/>
<define name="OFF_ITERATIONS" value="1" description="Farneback OF setting"/>
<define name="OFF_POLY_N" value="7" description="Farneback OF setting"/>
<define name="OFF_POLY_SIGMA" value="1.2" description="Farneback OF setting"/>
<define name="OFF_FLAGS" value="0" description="Farneback OF setting"/>

<!-- Divergence threshold -->
<define name="DET_THRESHOLD" value="0.14" description="Divergence cutoff threshold"/>

</module>
</firmware>

<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="6000"/>
</commands>

<servos driver="Default">
<servo name="TOP_LEFT" no="0" min="3000" neutral="3000" max="9800" />
<servo name="TOP_RIGHT" no="1" min="3000" neutral="3000" max="9800" />
<servo name="BOTTOM_RIGHT" no="2" min="3000" neutral="3000" max="9800" />
<servo name="BOTTOM_LEFT" no="3" min="3000" neutral="3000" max="9800" />
</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="TYPE" value="QUAD_X"/>
</section>

<command_laws>
<call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
<set servo="TOP_LEFT" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
<set servo="TOP_RIGHT" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
</command_laws>

<section name="BEBOP_FRONT_CAMERA" prefix="MT9F002_">
<define name="OUTPUT_HEIGHT" value="520" />
<define name="OUTPUT_WIDTH" value="240" />
<define name="OFFSET_X" value="0.09" />
<define name="ZOOM" value="1.25"/>
<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="30" />
</section>

<section name="AIR_DATA" prefix="AIR_DATA_">
<define name="CALC_AIRSPEED" value="FALSE"/>
<define name="CALC_TAS_FACTOR" value="FALSE"/>
<define name="CALC_AMSL_BARO" value="TRUE"/>
</section>

<section name="IMU" prefix="IMU_">
<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"/>
<!-- Magneto calibration -->
<define name="MAG_X_NEUTRAL" value="0"/>
<define name="MAG_Y_NEUTRAL" value="0"/>
<define name="MAG_Z_NEUTRAL" value="0"/>
</section>

<!-- local magnetic field -->
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
<section name="AHRS" prefix="AHRS_">
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
<!-- Delft -->
<define name="H_X" value="0.3892503"/>
<define name="H_Y" value="0.0017972"/>
<define name="H_Z" value="0.9211303"/>

<define name="HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
<define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>
</section>

<section name="INS" prefix="INS_">
<define name="SONAR_MAX_RANGE" value="2.2"/>

<!-- Use GPS altitude measurments and set the R gain -->
<define name="USE_GPS_ALT" value="1"/>
<define name="VFF_R_GPS" value="0.01"/>
</section>

<section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoint limits for attitude stabilization rc flight -->
<define name="SP_MAX_PHI" value="45" unit="deg"/>
<define name="SP_MAX_THETA" value="45" unit="deg"/>
<define name="SP_MAX_R" value="120" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="50"/>
</section>

<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
<!-- attitude reference generation model -->
<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.)"/>
</section>

<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness -->
<define name="G1_P" value="0.0397"/>
<define name="G1_Q" value="0.0299"/>
<define name="G1_R" value="0.0014"/>
<define name="G2_R" value="0.1219"/>

<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="600.0"/>
<define name="REF_ERR_Q" value="600.0"/>
<define name="REF_ERR_R" value="600.0"/>
<define name="REF_RATE_P" value="28.0"/>
<define name="REF_RATE_Q" value="28.0"/>
<define name="REF_RATE_R" value="28.0"/>

<!--Maxium yaw rate, to avoid instability-->
<define name="MAX_R" value="120.0" unit="deg/s"/>

<!-- second order filter parameters -->
<define name="FILT_CUTOFF" value="8.0"/>
<define name="ESTIMATION_FILT_CUTOFF" value="8.0"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
<define name="ACT_DYN_Q" value="0.1"/>
<define name="ACT_DYN_R" value="0.1"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</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="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.68"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>

<define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.68" />
<define name="ADAPT_MIN_HOVER_THROTTLE" value="0.55" />
<define name="ADAPT_MAX_HOVER_THROTTLE" value="0.72" />
<!-- <define name="ADAPT_NOISE_FACTOR" value="0.8" /> -->
</section>

<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<!-- Good weather -->
<define name="MAX_BANK" value="20" unit="deg"/>
<define name="REF_MAX_SPEED" value="0.5" unit="m/s"/>
<!-- Bad weather -->
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
<define name="PGAIN" value="120"/>
<define name="DGAIN" value="230"/>
<define name="IGAIN" value="40"/>
</section>

<section name="NAVIGATION" prefix="NAV_">
<define name="CLIMB_VSPEED" value="1.0"/>
<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="bebop" type="string"/>
<define name="MT9F002_SENSOR_RES_DIVIDER" value="4"/>
</section>
<include href="conf/simulator/gazebo/airframes/bebop.xml"/>

<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_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>

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

<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.9" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="10.8" unit="V"/>
<define name="LOW_BAT_LEVEL" value="11.1" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
27 changes: 27 additions & 0 deletions conf/modules/object_avoider.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
<!DOCTYPE module SYSTEM "module.dtd">

<module name="object_avoider" dir="of_avoidance">
<doc>
<description>
This file includes the autopilot functions from the module `of_avoidance` made by team 2 in 2023 for the AFMAV course.
</description>
<define name="OFF_DIV_SAFE_INDEX" value="37" description="The DIVERGENCE_SAFE_HEADING message channel"/>
<!-- Autopilot settings -->
<define name="N_DIRBLOCKS" value="5" description="Possible heading directions"/>
<define name="FOV_ANGLE" value="160" description="Determines the size of the steering correction when detecting an obstacle"/>
<define name="DIR_CHANGE_THRESHOLD" value="8" description="Number of consecutive measurements necessary to decide to steer away"/>
<define name="PAUSE_TIME" value="20" description="The confidence values reset and aren't updated for (PAUSE_TIME * 100ms) after a steer correction"/>
<define name="PAUSE_V_FACTOR" value="0.4" description="The percentage of the default speed it has when pausing [0.0 - 1.0]"/>
</doc>
<dep>
<depends>opencv_optical_flow</depends>
</dep>
<header>
<file name="object_avoider.h"/>
</header>
<init fun="object_avoider_init()"/>
<periodic fun="object_avoider_periodic()" freq="4"/>
<makefile target="ap|nps">
<file name="object_avoider.c"/>
</makefile>
</module>
Loading