mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
[modules] add optical_flow_landing
I added an optical flow landing module for rotorcrafts. It uses the size divergence from the optical flow algorithm to keep a constant divergence (if set negative the drone will land, 0 = hover, and positive is going up). Advantage with respect to a GPS landing is that it will take into account the landing surface, even if it is higher than the ground level (so landing on a roof for instance). The module does not use sonar for the vertical control, so it can be used by tiny drones that do not carry sonar. The module uses a novel theory on seeing distances with just a single camera (no extra sensors) to detect when it should land. So it can also be used by drones to estimate distance with a single camera. closes #1611
This commit is contained in:
guidoAI
Felix Ruess
@@ -62,6 +62,7 @@
|
||||
<field name="flow_der_x" type="int16_t">The derotated flow calculation in the x direction (in subpixels)</field>
|
||||
<field name="flow_der_y" type="int16_t">The derotated flow calculation in the y direction (in subpixels)</field>
|
||||
<field name="quality" type="uint8_t"/>
|
||||
<field name="size_divergence" type="float">Divergence as determined with the size method (in 1/seconds)</field>
|
||||
<field name="dist" type="float">Distance to ground</field>
|
||||
</message>
|
||||
|
||||
|
||||
@@ -0,0 +1,213 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<airframe name="ardrone2_optitrack">
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<target name="ap" board="ardrone2">
|
||||
<configure name="USE_BARO_BOARD" value="FALSE"/>
|
||||
</target>
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<module name="fdm" type="jsbsim"/>
|
||||
</target>
|
||||
|
||||
<define name="FAILSAFE_DESCENT_SPEED" value="0.5"/>
|
||||
<define name="USE_SONAR" value="0"/>
|
||||
<configure name="USE_MAGNETOMETER" value="0"/>
|
||||
|
||||
<!-- Subsystem section -->
|
||||
<module name="telemetry" type="transparent_udp"/>
|
||||
<module name="radio_control" type="datalink"/>
|
||||
<module name="motor_mixing"/>
|
||||
<module name="actuators" type="ardrone2"/>
|
||||
<module name="imu" type="ardrone2"/>
|
||||
<module name="gps" type="datalink"/>
|
||||
<module name="stabilization" type="indi"/>
|
||||
<module name="ahrs" type="int_cmpl_quat"/>
|
||||
<module name="ins" type="extended"/>
|
||||
</firmware>
|
||||
|
||||
<modules main_freq="512">
|
||||
<load name="bat_voltage_ardrone2.xml"/>
|
||||
<load name="stereocam_droplet.xml" />
|
||||
<load name="cv_opticflow.xml"/>
|
||||
<load name="optical_flow_landing.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"/>
|
||||
|
||||
<!-- Time cross layout (X), with order NW (CW), NE (CCW), SE (CW), SW (CCW) -->
|
||||
<define name="TYPE" value="QUAD_X"/>
|
||||
</section>
|
||||
|
||||
<command_laws>
|
||||
<call fun="motor_mixing_run(autopilot_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="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_">
|
||||
<!-- Delft -->
|
||||
<define name="H_X" value="0.3892503"/>
|
||||
<define name="H_Y" value="0.0017972"/>
|
||||
<define name="H_Z" value="0.9211303"/>
|
||||
|
||||
<!-- Use GPS heading instead of magneto -->
|
||||
<define name="USE_GPS_HEADING" value="1"/>
|
||||
<define name="HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<!-- 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="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||
<!-- control effectiveness -->
|
||||
<define name="G1_P" value="0.028551973"/>
|
||||
<define name="G1_Q" value="0.023775417"/>
|
||||
<define name="G1_R" value="0.00173069"/>
|
||||
<define name="G2_R" value="0.086460732"/>
|
||||
|
||||
<!-- reference acceleration for attitude control -->
|
||||
<define name="REF_ERR_P" value="380.0"/>
|
||||
<define name="REF_ERR_Q" value="380.0"/>
|
||||
<define name="REF_ERR_R" value="70.0"/>
|
||||
<define name="REF_RATE_P" value="21.6"/>
|
||||
<define name="REF_RATE_Q" value="21.6"/>
|
||||
<define name="REF_RATE_R" value="11.0"/>
|
||||
|
||||
<!-- second order filter parameters -->
|
||||
<define name="FILT_OMEGA" value="20.0"/>
|
||||
<define name="FILT_ZETA" value="0.55"/>
|
||||
<define name="FILT_OMEGA_R" value="20.0"/>
|
||||
<define name="FILT_ZETA_R" value="0.55"/>
|
||||
|
||||
<!-- first order actuator dynamics -->
|
||||
<define name="ACT_DYN_P" value="0.06"/>
|
||||
<define name="ACT_DYN_Q" value="0.06"/>
|
||||
<define name="ACT_DYN_R" value="0.06"/>
|
||||
|
||||
<!-- Adaptive Learning Rate -->
|
||||
<define name="USE_ADAPTIVE" value="FALSE"/>
|
||||
<define name="ADAPTIVE_MU" value="0.0001"/>
|
||||
</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="180" 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="450" unit="deg/s"/>
|
||||
<define name="REF_ZETA_R" value="0.9"/>
|
||||
<define name="REF_MAX_R" value="600." unit="deg/s"/>
|
||||
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<define name="HOVER_KP" value="583"/>
|
||||
<define name="HOVER_KD" value="82"/>
|
||||
<define name="HOVER_KI" value="13"/>
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value="0.645"/>
|
||||
<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="363"/>
|
||||
<define name="DGAIN" value="237"/>
|
||||
<define name="IGAIN" value="30"/>
|
||||
<define name="VGAIN" value="0"/>
|
||||
<define name="AGAIN" value="0"/>
|
||||
</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_ardrone2" type="string"/>
|
||||
<define name="JSBSIM_INIT" value="reset00" type="string"/>
|
||||
<define name="SENSORS_PARAMS" value="nps_sensors_params_ardrone2.h" type="string"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<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="ARRIVED_AT_WAYPOINT" value="0.15"/>
|
||||
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="1500"/>
|
||||
<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.3" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="9.9" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
|
||||
</section>
|
||||
</airframe>
|
||||
@@ -0,0 +1,13 @@
|
||||
<conf>
|
||||
<aircraft
|
||||
name="ARDrone2_Guido_optitrack"
|
||||
ac_id="29"
|
||||
airframe="airframes/TUDELFT/tudelft_guido_ardrone2_optitrack.xml"
|
||||
radio="radios/dummy.xml"
|
||||
telemetry="telemetry/default_rotorcraft.xml"
|
||||
flight_plan="flight_plans/rotorcraft_guido_optitrack.xml"
|
||||
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/estimation/body_to_imu.xml settings/control/stabilization_indi.xml"
|
||||
settings_modules="modules/cv_opticflow.xml modules/optical_flow_landing.xml"
|
||||
gui_color="#fffff996b847"
|
||||
/>
|
||||
</conf>
|
||||
@@ -0,0 +1,241 @@
|
||||
<control_panel name="paparazzi control panel">
|
||||
<section name="programs">
|
||||
<program name="Server" command="sw/ground_segment/tmtc/server"/>
|
||||
<program name="Data Link" command="sw/ground_segment/tmtc/link">
|
||||
<arg flag="-d" constant="/dev/ttyUSB0"/>
|
||||
</program>
|
||||
<program name="Link Combiner" command="sw/ground_segment/python/redundant_link/link_combiner.py"/>
|
||||
<program name="GCS" command="sw/ground_segment/cockpit/gcs"/>
|
||||
<program name="Flight Plan Editor" command="sw/ground_segment/cockpit/gcs">
|
||||
<arg flag="-edit"/>
|
||||
</program>
|
||||
<program name="Messages" command="sw/ground_segment/tmtc/messages"/>
|
||||
<program name="Messages (Python)" command="sw/ground_segment/python/messages_app/messagesapp.py"/>
|
||||
<program name="Settings" command="sw/ground_segment/tmtc/settings">
|
||||
<arg flag="-ac" constant="@AIRCRAFT"/>
|
||||
</program>
|
||||
<program name="Settings (Python)" command="sw/ground_segment/python/settings_app/settingsapp.py"/>
|
||||
<program name="GPSd position display" command="sw/ground_segment/tmtc/gpsd2ivy"/>
|
||||
<program name="Log Plotter" command="sw/logalizer/logplotter"/>
|
||||
<program name="Real-time Plotter" command="sw/logalizer/plotter"/>
|
||||
<program name="Real-time Plotter (Python)" command="sw/ground_segment/python/real_time_plot/messagepicker.py"/>
|
||||
<program name="Log File Player" command="sw/logalizer/play"/>
|
||||
<program name="Simulator" command="sw/simulator/pprzsim-launch"/>
|
||||
<program name="Video Synchronizer" command="sw/ground_segment/misc/video_synchronizer"/>
|
||||
<program name="Joystick" command="sw/ground_segment/joystick/input2ivy">
|
||||
<arg flag="-ac" constant="@AIRCRAFT"/>
|
||||
<arg flag="xbox_gamepad.xml"/>
|
||||
</program>
|
||||
<program name="Hardware in the Loop" command="sw/simulator/simhitl"/>
|
||||
<program name="Environment Simulator" command="sw/simulator/gaia"/>
|
||||
<program name="Http Server" command="$python">
|
||||
<arg flag="-m" constant="SimpleHTTPServer"/>
|
||||
<arg flag="8889"/>
|
||||
</program>
|
||||
<program name="Plot Meteo Profile" command="sw/logalizer/plotprofile"/>
|
||||
<program name="Weather Station" command="sw/ground_segment/misc/davis2ivy">
|
||||
<arg flag="-d" constant="/dev/ttyUSB1"/>
|
||||
</program>
|
||||
<program name="Attitude Visualizer" command="sw/tools/attitude_viz.py"/>
|
||||
<program name="App Server" command="sw/ground_segment/tmtc/app_server"/>
|
||||
<program name="NatNet" command="sw/ground_segment/misc/natnet2ivy"/>
|
||||
<program name="Ivy2Nmea" command="sw/ground_segment/tmtc/ivy2nmea">
|
||||
<arg flag="--port" constant="/dev/ttyUSB1"/>
|
||||
<arg flag="--id" constant="1"/>
|
||||
</program>
|
||||
<program name="BluegigaUsbDongleScanner" command="sw/tools/bluegiga_usb_dongle/bluegiga_usb_driver">
|
||||
<arg flag="/dev/ttyACM2"/>
|
||||
<arg flag="scan" />
|
||||
</program>
|
||||
<program name="BluegigaUsbDongle" command="sw/tools/bluegiga_usb_dongle/bluegiga_usb_driver">
|
||||
<arg flag="/dev/ttyACM2"/>
|
||||
<arg flag="00:07:00:2d:d6:bb" />
|
||||
<arg flag="4242" />
|
||||
<arg flag="4252" />
|
||||
</program>
|
||||
</section>
|
||||
|
||||
<section name="sessions">
|
||||
<session name="Bluegiga">
|
||||
<program name="Data Link">
|
||||
<arg flag="-udp"/>
|
||||
<arg flag="-udp_port" constant="4242"/>
|
||||
<arg flag="-udp_uplink_port" constant="4252"/>
|
||||
</program>
|
||||
<program name="Server"/>
|
||||
<program name="Messages"/>
|
||||
<program name="GCS">
|
||||
<arg flag="-speech" />
|
||||
</program>
|
||||
<program name="NatNet">
|
||||
<arg flag="-ac" constant="1"/>
|
||||
<arg flag="12"/>
|
||||
<arg flag="-small"/>
|
||||
<arg flag="-tf" constant="5"/>
|
||||
</program>
|
||||
<program name="Joystick">
|
||||
<arg flag="-d" constant="2"/>
|
||||
<arg flag="-ac" constant="@AIRCRAFT"/>
|
||||
<arg flag="hobbyking.xml"/>
|
||||
</program>
|
||||
</session>
|
||||
<session name="Guido">
|
||||
<program name="Data Link">
|
||||
<arg flag="-udp"/>
|
||||
</program>
|
||||
<program name="Server"/>
|
||||
<program name="GCS">
|
||||
<arg flag="-speech"/>
|
||||
</program>
|
||||
<program name="Joystick">
|
||||
<arg flag="-ac" constant="ARDrone2_optitrack"/>
|
||||
<arg flag="hobbyking.xml"/>
|
||||
<arg flag="-d" constant="1"/>
|
||||
</program>
|
||||
<program name="NatNet">
|
||||
<arg flag="-ac" constant="2"/>
|
||||
<arg flag="11"/>
|
||||
</program>
|
||||
</session>
|
||||
<session name="Flight USB0 @57600">
|
||||
<program name="Data Link">
|
||||
<arg flag="-d" constant="/dev/ttyUSB0"/>
|
||||
<arg flag="-s" constant="57600"/>
|
||||
</program>
|
||||
<program name="Server"/>
|
||||
<program name="GCS">
|
||||
<arg flag="-speech" />
|
||||
</program>
|
||||
</session>
|
||||
<session name="Flight Paparazzi @57600">
|
||||
<program name="Data Link">
|
||||
<arg flag="-d" constant="/dev/paparazzi/link"/>
|
||||
<arg flag="-s" constant="57600"/>
|
||||
</program>
|
||||
<program name="Server"/>
|
||||
<program name="GCS">
|
||||
<arg flag="-speech" />
|
||||
</program>
|
||||
</session>
|
||||
<session name="Flight ACM1 @57600">
|
||||
<program name="Data Link">
|
||||
<arg flag="-d" constant="/dev/ttyACM1"/>
|
||||
<arg flag="-s" constant="57600"/>
|
||||
</program>
|
||||
<program name="Server"/>
|
||||
<program name="GCS">
|
||||
<arg flag="-speech" />
|
||||
</program>
|
||||
</session>
|
||||
<session name="Flight ACM0 @57600">
|
||||
<program name="Data Link">
|
||||
<arg flag="-d" constant="/dev/ttyACM0"/>
|
||||
<arg flag="-s" constant="57600"/>
|
||||
</program>
|
||||
<program name="Server"/>
|
||||
<program name="GCS">
|
||||
<arg flag="-speech" />
|
||||
</program>
|
||||
</session>
|
||||
<session name="Flight XBee @57600">
|
||||
<program name="Data Link">
|
||||
<arg flag="-d" constant="/dev/paparazzi/xbee"/>
|
||||
<arg flag="-transport" constant="xbee"/>
|
||||
<arg flag="-s" constant="57600"/>
|
||||
</program>
|
||||
<program name="Server"/>
|
||||
<program name="GCS">
|
||||
<arg flag="-speech" />
|
||||
</program>
|
||||
</session>
|
||||
<session name="Flight UDP">
|
||||
<program name="Data Link">
|
||||
<arg flag="-udp"/>
|
||||
</program>
|
||||
<program name="Server"/>
|
||||
<program name="GCS">
|
||||
<arg flag="-speech" />
|
||||
</program>
|
||||
</session>
|
||||
<session name="Raw Sensors">
|
||||
<program name="Real-time Plotter">
|
||||
<arg flag="-g" constant="1000x250-0+0"/>
|
||||
<arg flag="-t" constant="ACC"/>
|
||||
<arg flag="-u" constant="0.05"/>
|
||||
<arg flag="-c" constant="0.00"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_RAW:ax"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_RAW:ay"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_RAW:az"/>
|
||||
<arg flag="-n"/>
|
||||
<arg flag="-g" constant="1000x250-0+250"/>
|
||||
<arg flag="-t" constant="GYRO"/>
|
||||
<arg flag="-u" constant="0.05"/>
|
||||
<arg flag="-c" constant="0.00"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_GYRO_RAW:gp"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_GYRO_RAW:gq"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_GYRO_RAW:gr"/>
|
||||
<arg flag="-n"/>
|
||||
<arg flag="-g" constant="1000x250-0+500"/>
|
||||
<arg flag="-t" constant="MAG"/>
|
||||
<arg flag="-u" constant="0.05"/>
|
||||
<arg flag="-c" constant="0.00"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_MAG_RAW:mx"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_MAG_RAW:my"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_MAG_RAW:mz"/>
|
||||
<arg flag="-n"/>
|
||||
<arg flag="-g" constant="1000x250-0+750"/>
|
||||
<arg flag="-t" constant="BARO"/>
|
||||
<arg flag="-u" constant="0.05"/>
|
||||
<arg flag="-c" constant="101325.0"/>
|
||||
<arg flag="-c" constant="*:telemetry:BARO_RAW:abs"/>
|
||||
</program>
|
||||
</session>
|
||||
<session name="Scaled Sensors">
|
||||
<program name="Real-time Plotter">
|
||||
<arg flag="-g" constant="1000x250-0+0"/>
|
||||
<arg flag="-t" constant="ACC"/>
|
||||
<arg flag="-u" constant="0.05"/>
|
||||
<arg flag="-c" constant="9.81"/>
|
||||
<arg flag="-c" constant="0.00"/>
|
||||
<arg flag="-c" constant="-9.81"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:ax:0.0009766"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:ay:0.0009766"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_ACCEL_SCALED:az:0.0009766"/>
|
||||
<arg flag="-n"/>
|
||||
<arg flag="-g" constant="1000x250-0+250"/>
|
||||
<arg flag="-t" constant="GYRO"/>
|
||||
<arg flag="-u" constant="0.05"/>
|
||||
<arg flag="-c" constant="0.00"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gp:0.0139882"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gq:0.0139882"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_GYRO_SCALED:gr:0.0139882"/>
|
||||
<arg flag="-n"/>
|
||||
<arg flag="-g" constant="1000x250-0+500"/>
|
||||
<arg flag="-t" constant="MAG"/>
|
||||
<arg flag="-u" constant="0.05"/>
|
||||
<arg flag="-c" constant="0.00"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:mx:0.0004883"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:my:0.0004883"/>
|
||||
<arg flag="-c" constant="*:telemetry:IMU_MAG_SCALED:mz:0.0004883"/>
|
||||
</program>
|
||||
</session>
|
||||
<session name="Messages and Settings">
|
||||
<program name="Messages"/>
|
||||
<program name="Settings">
|
||||
<arg flag="-ac" constant="@AIRCRAFT"/>
|
||||
</program>
|
||||
</session>
|
||||
<session name="Joystick Hobbyking">
|
||||
<program name="Joystick">
|
||||
<arg flag="-ac" constant="@AIRCRAFT"/>
|
||||
<arg flag="hobbyking.xml"/>
|
||||
</program>
|
||||
</session>
|
||||
<session name="Joystick X3D Pro">
|
||||
<program name="Joystick">
|
||||
<arg flag="-ac" constant="@AIRCRAFT"/>
|
||||
<arg flag="extreme_3d_pro.xml"/>
|
||||
</program>
|
||||
</session>
|
||||
</section>
|
||||
</control_panel>
|
||||
@@ -0,0 +1,97 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="1.5" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="20" name="Rotorcraft Optitrack (Delft)" security_height="0.3">
|
||||
<header>
|
||||
#include "autopilot.h"
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0.0" y="0.0"/>
|
||||
<waypoint name="CLIMB" x="1.2" y="-0.6"/>
|
||||
<waypoint name="GOAL" x="2.0" y="2.0"/>
|
||||
<waypoint name="STDBY" x="-0.7" y="-0.8"/>
|
||||
<waypoint name="TD" x="0.8" y="-1.7"/>
|
||||
<waypoint lat="51.990630" lon="4.376823" name="p1"/>
|
||||
<waypoint lat="51.990614" lon="4.376779" name="p2"/>
|
||||
<waypoint lat="51.990636" lon="4.376759" name="p3"/>
|
||||
<waypoint lat="51.990651" lon="4.376805" name="p4"/>
|
||||
<waypoint lat="51.9906213" lon="4.3768628" name="FA1"/>
|
||||
<waypoint lat="51.9905874" lon="4.3767766" name="FA2"/>
|
||||
<waypoint lat="51.9906409" lon="4.3767226" name="FA3"/>
|
||||
<waypoint lat="51.9906737" lon="4.3768074" name="FA4"/>
|
||||
</waypoints>
|
||||
<sectors>
|
||||
<sector color="red" name="Flight_Area">
|
||||
<corner name="FA4"/>
|
||||
<corner name="FA3"/>
|
||||
<corner name="FA2"/>
|
||||
<corner name="FA1"/>
|
||||
</sector>
|
||||
</sectors>
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
<call fun="NavKillThrottle()"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
<block name="Holding point">
|
||||
<call fun="NavKillThrottle()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Start Engine">
|
||||
<call fun="NavResurrect()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
|
||||
<exception cond="stateGetPositionEnu_f()->z > 1.0" deroute="Standby"/>
|
||||
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
|
||||
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
|
||||
</block>
|
||||
<block name="Standby" strip_button="Standby" strip_icon="home.png">
|
||||
<stay wp="STDBY"/>
|
||||
</block>
|
||||
<block name="START" strip_button="Go" strip_icon="lookfore.png">
|
||||
<call fun="NavSetWaypointHere(WP_GOAL)"/>
|
||||
</block>
|
||||
<block name="StayGoal">
|
||||
<stay wp="GOAL"/>
|
||||
</block>
|
||||
<block name="stay_p1">
|
||||
<stay wp="p1"/>
|
||||
</block>
|
||||
<block name="go_p2">
|
||||
<call fun="nav_set_heading_deg(90)"/>
|
||||
<go wp="p2"/>
|
||||
<deroute block="stay_p1"/>
|
||||
</block>
|
||||
<block name="line_p1_p2">
|
||||
<go from="p1" hmode="route" wp="p2"/>
|
||||
<stay until="stage_time>10" wp="p2"/>
|
||||
<go from="p2" hmode="route" wp="p1"/>
|
||||
<deroute block="stay_p1"/>
|
||||
</block>
|
||||
<block name="route">
|
||||
<go from="p1" hmode="route" wp="p3"/>
|
||||
<go from="p3" hmode="route" wp="p4"/>
|
||||
<go from="p4" hmode="route" wp="p1"/>
|
||||
<deroute block="stay_p1"/>
|
||||
</block>
|
||||
<block name="Oval">
|
||||
<oval p1="p1" p2="p2" radius="-1"/>
|
||||
</block>
|
||||
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
|
||||
<call fun="NavSetWaypointHere(WP_TD)"/>
|
||||
</block>
|
||||
<block name="land">
|
||||
<go wp="TD"/>
|
||||
</block>
|
||||
<block name="flare">
|
||||
<exception cond="NavDetectGround()" deroute="Holding point"/>
|
||||
<exception cond="!nav_is_in_flight()" deroute="landed"/>
|
||||
<call fun="NavStartDetectGround()"/>
|
||||
<stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
|
||||
</block>
|
||||
<block name="landed">
|
||||
<call fun="NavKillThrottle()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -0,0 +1,52 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="optical_flow_landing" dir="ctrl">
|
||||
<doc>
|
||||
<description>
|
||||
Optical flow landing.
|
||||
|
||||
This module implements optical flow landings in which the divergence is kept constant (for rotorcraft). When using a fixed gain for control, the covariance between thrust and divergence is tracked, so that the drone knows when it has arrived close to the landing surface. Then, a final landing procedure is triggered. It can also be set to adaptive gain control, where the goal is to continuously gauge the distance to the landing surface. In this mode, the drone will oscillate all the way down to the surface.
|
||||
|
||||
de Croon, G.C.H.E. (2016). Monocular distance estimation with optical flow maneuvers and efference copies: a stability-based strategy. Bioinspiration and Biomimetics, 11(1), 016004.
|
||||
</description>
|
||||
<define name="OPTICAL_FLOW_LANDING_AGL_ID" value="ABI_BROADCAST" description="Sender id of the AGL (sonar) ABI message"/>
|
||||
<section name="OPTICAL_FLOW_LANDING" prefix="OPTICAL_FLOW_LANDING_">
|
||||
<define name="PGAIN" value="0.40" description="P gain on height error"/>
|
||||
<define name="IGAIN" value="0.00" description="I gain on summed height error"/>
|
||||
<define name="DGAIN" value="0.00" description="D gain on summed height error"/>
|
||||
<define name="VISION_METHOD" value="1" description="0 = fake vision, 1 = real vision"/>
|
||||
<define name="CONTROL_METHOD" value="0" description="0 = fixed gain control, 1 = adaptive gain control"/>
|
||||
<define name="COV_METHOD" value="0" description="0 = cov(uz, div), 1 = cov(div_past, div)"/>
|
||||
</section>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings NAME="OpticalFlowLanding">
|
||||
<dl_setting var="of_landing_ctrl.nominal_thrust" min="0" step="0.001" max="1" module="ctrl/optical_flow_landing" shortname="nominal_thrust"/>
|
||||
<dl_setting var="of_landing_ctrl.lp_factor" min="0" step="0.01" max="1" module="ctrl/optical_flow_landing" shortname="lp_factor"/>
|
||||
<dl_setting var="of_landing_ctrl.divergence_setpoint" min="-1" step="0.01" max="1" module="ctrl/optical_flow_landing" shortname="div sp"/>
|
||||
<dl_setting var="of_landing_ctrl.cov_set_point" min="-0.50" step="0.0001" max="0.50" module="ctrl/optical_flow_landing" shortname="cov_set_point"/>
|
||||
<dl_setting var="of_landing_ctrl.cov_limit" min="0" step="0.0001" max="3" module="ctrl/optical_flow_landing" shortname="cov_limit"/>
|
||||
<dl_setting var="of_landing_ctrl.pgain" min="0" step="0.001" max="6" module="ctrl/optical_flow_landing" shortname="pgain" param="OPTICAL_FLOW_LANDING_PGAIN"/>
|
||||
<dl_setting var="of_landing_ctrl.igain" min="0" step="0.001" max="1" module="ctrl/optical_flow_landing" shortname="igain" param="OPTICAL_FLOW_LANDING_IGAIN"/>
|
||||
<dl_setting var="of_landing_ctrl.dgain" min="0" step="0.001" max="1" module="ctrl/optical_flow_landing" shortname="dgain" param="OPTICAL_FLOW_LANDING_DGAIN"/>
|
||||
<dl_setting var="of_landing_ctrl.VISION_METHOD" min="0" step="1" max="1" module="ctrl/optical_flow_landing" shortname="VISION_METHOD" param="OPTICAL_FLOW_LANDING_VISION_METHOD"/>
|
||||
<dl_setting var="of_landing_ctrl.CONTROL_METHOD" min="0" step="1" max="1" module="ctrl/optical_flow_landing" shortname="CONTROL_METHOD" param="OPTICAL_FLOW_LANDING_CONTROL_METHOD"/>
|
||||
<dl_setting var="of_landing_ctrl.COV_METHOD" min="0" step="1" max="1" module="ctrl/optical_flow_landing" shortname="COV_METHOD" param="OPTICAL_FLOW_LANDING_COV_METHOD"/>
|
||||
<dl_setting var="of_landing_ctrl.delay_steps" min="0" step="1" max="60" module="ctrl/optical_flow_landing" shortname="delay_steps"/>
|
||||
<dl_setting var="of_landing_ctrl.pgain_adaptive" min="0" step="0.1" max="20.0" module="ctrl/optical_flow_landing" shortname="pgain_adaptive"/>
|
||||
<dl_setting var="of_landing_ctrl.igain_adaptive" min="0" step="0.01" max="1.0" module="ctrl/optical_flow_landing" shortname="igain_adaptive"/>
|
||||
<dl_setting var="of_landing_ctrl.dgain_adaptive" min="0" step="0.01" max="5.0" module="ctrl/optical_flow_landing" shortname="dgain_adaptive"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
|
||||
<header>
|
||||
<file name="optical_flow_landing.h"/>
|
||||
</header>
|
||||
|
||||
<makefile target="ap">
|
||||
<file name="optical_flow_landing.c"/>
|
||||
</makefile>
|
||||
|
||||
</module>
|
||||
@@ -172,6 +172,7 @@ void opticflow_module_run(void)
|
||||
opticflow_result.flow_der_x,
|
||||
opticflow_result.flow_der_x,
|
||||
quality,
|
||||
opticflow_result.div_size,
|
||||
opticflow_state.agl);
|
||||
//TODO Find an appropiate quality measure for the noise model in the state filter, for now it is tracked_cnt
|
||||
if (opticflow_result.tracked_cnt > 0) {
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,91 @@
|
||||
/*
|
||||
* Copyright (C) 2016
|
||||
*
|
||||
* 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, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/ctrl/optical_flow_landing.c
|
||||
* @brief This module implements optical flow landings in which the divergence is kept constant.
|
||||
* When using a fixed gain for control, the covariance between thrust and divergence is tracked,
|
||||
* so that the drone knows when it has arrived close to the landing surface. Then, a final landing
|
||||
* procedure is triggered. It can also be set to adaptive gain control, where the goal is to continuously
|
||||
* gauge the distance to the landing surface. In this mode, the drone will oscillate all the way down to
|
||||
* the surface.
|
||||
*
|
||||
* de Croon, G.C.H.E. (2016). Monocular distance estimation with optical flow maneuvers and efference copies:
|
||||
* a stability-based strategy. Bioinspiration & biomimetics, 11(1), 016004.
|
||||
* <http://iopscience.iop.org/article/10.1088/1748-3190/11/1/016004>
|
||||
*/
|
||||
|
||||
#ifndef OPTICAL_FLOW_LANDING_H_
|
||||
#define OPTICAL_FLOW_LANDING_H_
|
||||
|
||||
// number of time steps used for calculating the covariance (oscillations)
|
||||
#define COV_WINDOW_SIZE 60
|
||||
|
||||
#include "std.h"
|
||||
|
||||
|
||||
struct OpticalFlowLanding {
|
||||
float agl; ///< agl = height from sonar (only used when using "fake" divergence)
|
||||
float agl_lp; ///< low-pass version of agl
|
||||
float lp_factor; ///< low-pass factor in [0,1], with 0 purely using the current measurement
|
||||
float vel; ///< vertical velocity as determined with sonar (only used when using "fake" divergence)
|
||||
float divergence_setpoint; ///< setpoint for constant divergence approach
|
||||
float pgain; ///< P-gain for constant divergence control (from divergence error to thrust)
|
||||
float igain; ///< I-gain for constant divergence control
|
||||
float dgain; ///< D-gain for constant divergence control
|
||||
float sum_err; ///< integration of the error for I-gain
|
||||
float nominal_thrust; ///< nominal thrust around which the PID-control operates
|
||||
int VISION_METHOD; ///< whether to use vision (1) or Optitrack / sonar (0)
|
||||
int CONTROL_METHOD; ///< type of divergence control: 0 = fixed gain, 1 = adaptive gain
|
||||
float cov_set_point; ///< for adaptive gain control, setpoint of the covariance (oscillations)
|
||||
float cov_limit; ///< for fixed gain control, what is the cov limit triggering the landing
|
||||
float pgain_adaptive; ///< P-gain for adaptive gain control
|
||||
float igain_adaptive; ///< I-gain for adaptive gain control
|
||||
float dgain_adaptive; ///< D-gain for adaptive gain control
|
||||
int COV_METHOD; ///< method to calculate the covariance: between thrust and div (0) or div and div past (1)
|
||||
int delay_steps; ///< number of delay steps for div past
|
||||
};
|
||||
|
||||
extern struct OpticalFlowLanding of_landing_ctrl;
|
||||
|
||||
// arrays containing histories for determining covariance
|
||||
float thrust_history[COV_WINDOW_SIZE];
|
||||
float divergence_history[COV_WINDOW_SIZE];
|
||||
float past_divergence_history[COV_WINDOW_SIZE];
|
||||
unsigned long ind_hist;
|
||||
|
||||
// Without optitrack set to: GUIDANCE_H_MODE_ATTITUDE
|
||||
// With optitrack set to: GUIDANCE_H_MODE_HOVER
|
||||
#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_HOVER
|
||||
|
||||
// Own guidance_v
|
||||
#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE
|
||||
|
||||
// supporting functions for cov calculation:
|
||||
float get_cov(float *a, float *b, int n_elements);
|
||||
float get_mean_array(float *a, int n_elements);
|
||||
void reset_all_vars(void);
|
||||
|
||||
// Implement own Vertical loops
|
||||
extern void guidance_v_module_init(void);
|
||||
extern void guidance_v_module_enter(void);
|
||||
extern void guidance_v_module_run(bool in_flight);
|
||||
|
||||
#endif /* OPTICAL_FLOW_LANDING_H_ */
|
||||
+1
-1
Submodule sw/ext/pprzlink updated: fceb463aed...c915a014e0
Reference in New Issue
Block a user