mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
Monocular Optical Flow hover (#2239)
This commit is contained in:
Titus
committed by
Gautier Hattenberger
Gautier Hattenberger
parent
a628e6af97
commit
30ab122e91
@@ -0,0 +1,252 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<airframe name="ardrone2">
|
||||
<description>
|
||||
This airframe uses the optical flow module that uses monocular vision height estimation to stabilize a quadrotor using only a downwards
|
||||
facing camera and an IMU. For additional documentation please visit the module xml file.
|
||||
|
||||
The settings in this airframe are calibrated for performance on an ARDrone2 specifically.
|
||||
</description>
|
||||
<firmware name="rotorcraft">
|
||||
<target name="ap" board="ardrone2">
|
||||
<configure name="USE_BARO_BOARD" value="FALSE" />
|
||||
</target>
|
||||
<define name="USE_SONAR" value="FALSE" />
|
||||
|
||||
<!-- 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" />
|
||||
|
||||
<!-- Switch GPS for Optitrack mode -->
|
||||
<module name="gps" type="datalink" />
|
||||
|
||||
<module name="stabilization" type="indi_simple" />
|
||||
<module name="ahrs" type="int_cmpl_quat">
|
||||
<configure name="USE_MAGNETOMETER" value="0" />
|
||||
</module>
|
||||
|
||||
<!--module name="ins" type="extended">
|
||||
</module-->
|
||||
|
||||
<module name="ins" type="gps_passthrough"/>
|
||||
|
||||
<module name="bat_voltage_ardrone2" />
|
||||
<module name="video_thread" />
|
||||
<module name="pose_history" />
|
||||
<module name="cv_opticflow">
|
||||
<define name="OPTICFLOW_METHOD" value="0" />
|
||||
<define name="OPTICFLOW_CAMERA" value="bottom_camera" />
|
||||
<!-- ARDrone2 FPS improvements -->
|
||||
<define name="OPTICFLOW_PYRAMID_LEVEL" value="0"/>
|
||||
<define name="OPTICFLOW_FEATURE_MANAGEMENT" value="1"/>
|
||||
</module>
|
||||
|
||||
<module name="optical_flow_hover" >
|
||||
<define name="OFH_HOVER_METHOD" value = "1" />
|
||||
<define name="OFH_MAXBANK" value = "10.f" />
|
||||
<define name="XY_SYMMETRICAL" value = "1" />
|
||||
|
||||
<define name="OFH_OSCPHI" value = "1" />
|
||||
<define name="OFH_OSCTHETA" value = "0" />
|
||||
|
||||
<define name="COV_WINDOW_SIZE" value = "300" />
|
||||
<define name="OF_LP_CONST" value = "0.6" />
|
||||
|
||||
<define name="OFH_RAMPZ" value = "0.125" />
|
||||
<define name="OFH_IGAINZ" value = "0.003" />
|
||||
<define name="OFH_REDUCTIONZ" value = "0.45" />
|
||||
<define name="OFH_COVDIV_SETPOINT" value = "-0.025" />
|
||||
|
||||
<define name="OFH_IGAINX" value = "0.00005" />
|
||||
<define name="OFH_IGAINY" value = "0.00005" />
|
||||
<define name="OFH_RAMPXY" value = "0.0004" />
|
||||
<define name="OFH_REDUCTIONXY" value = "0.3" />
|
||||
<define name="OFH_COVFLOW_SETPOINT" value = "-2000.f" />
|
||||
|
||||
<define name="OFH_VER_SLOPE_A" value = "0.5" />
|
||||
<define name="OFH_VER_SLOPE_B" value = "0.25" />
|
||||
<define name="OFH_HOR_X_SLOPE_A" value = "0.006" />
|
||||
<define name="OFH_HOR_X_SLOPE_B" value = "0.004" />
|
||||
</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="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_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="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.0" unit="deg" />
|
||||
<define name="BODY_TO_IMU_THETA" value="3.0" unit="deg" />
|
||||
<define name="BODY_TO_IMU_PSI" value="0.0" unit="deg" />
|
||||
</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" />
|
||||
|
||||
<!-- 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" 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="450" 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.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_CUTOFF" value="3.2" />
|
||||
<define name="FILT_CUTOFF_R" value="3.2" />
|
||||
|
||||
<!-- 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.0002" />
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<!-- Agressive gains -->
|
||||
<define name="HOVER_KP" value="600" />
|
||||
<define name="HOVER_KD" value="400" />
|
||||
|
||||
<!-- Less Agressive Gains
|
||||
<define name="HOVER_KP" value="363" />
|
||||
<define name="HOVER_KD" value="237" /> -->
|
||||
|
||||
<define name="HOVER_KI" value="13" />
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value="0.61" />
|
||||
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE" />
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<define name="MAX_BANK" value="22" unit="deg" />
|
||||
<!-- Agressive Gains -->
|
||||
<define name="PGAIN" value="363" />
|
||||
<define name="DGAIN" value="237" />
|
||||
<define name="IGAIN" value="30" />
|
||||
|
||||
<!-- Less Agressive Gains >
|
||||
<define name="PGAIN" value="80" />
|
||||
<define name="DGAIN" value="40" />
|
||||
<define name="IGAIN" value="13" /> -->
|
||||
</section>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<define name="MODE_STARTUP" value="AP_MODE_ATTITUDE_DIRECT" />
|
||||
<define name="MODE_MANUAL" value="AP_MODE_MODULE" />
|
||||
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT" />
|
||||
<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.6" unit="V" />
|
||||
<define name="CRITIC_BAT_LEVEL" value="9.9" unit="V" />
|
||||
<define name="LOW_BAT_LEVEL" value="10.2" unit="V" />
|
||||
<define name="MAX_BAT_LEVEL" value="12.4" unit="V" />
|
||||
</section>
|
||||
</airframe>
|
||||
@@ -0,0 +1,229 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<airframe name="tudelft_tb_bebop">
|
||||
<firmware name="rotorcraft">
|
||||
<target name="ap" board="bebop"/>
|
||||
<define name="USE_SONAR" value="FALSE" />
|
||||
|
||||
<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"/> <!-- TURN ON OUTSIDE -->
|
||||
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
|
||||
</module>
|
||||
<!--module name="ins" type="extended">
|
||||
</module-->
|
||||
<module name="ins" type="gps_passthrough"/>
|
||||
<!-- module name="guidance" type="indi"/-->
|
||||
<!-- <module name="guidance" type="indi"> <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN"
|
||||
value="-500.0"/> <define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.1"/>
|
||||
<define name="GUIDANCE_INDI_RC_DEBUG" value="TRUE"/> </module> -->
|
||||
</firmware>
|
||||
<modules main_freq="512">
|
||||
<module name="video_thread"/>
|
||||
<module name="pose_history"/>
|
||||
<module name="cv_opticflow">
|
||||
<define name="OPTICFLOW_METHOD" value="0"/>
|
||||
<define name="OPTICFLOW_CAMERA" value="bottom_camera"/>
|
||||
<define name="OPTICFLOW_SEARCH_DISTANCE" value="40"/>
|
||||
</module>
|
||||
<module name="optical_flow_hover" >
|
||||
<define name="OFH_HOVER_METHOD" value = "1" />
|
||||
<define name="OFH_MAXBANK" value = "10.f" />
|
||||
<define name="XY_SYMMETRICAL" value = "1" />
|
||||
|
||||
<define name="OFH_OSCPHI" value = "1" />
|
||||
<define name="OFH_OSCTHETA" value = "0" />
|
||||
|
||||
<define name="OF_LP_CONST" value = "0.4" />
|
||||
<define name="COV_WINDOW_SIZE" value = "300" />
|
||||
|
||||
<define name="OFH_IGAINZ" value = "0.002" />
|
||||
<define name="OFH_RAMPZ" value = "0.15" />
|
||||
<define name="OFH_REDUCTIONZ" value = "0.45" />
|
||||
<define name="OFH_COVDIV_SETPOINT" value = "-0.02" />
|
||||
|
||||
<define name="OFH_IGAINX" value = "0.00002" />
|
||||
<define name="OFH_IGAINY" value = "0.00002" />
|
||||
<define name="OFH_RAMPXY" value = "0.0008" />
|
||||
<define name="OFH_REDUCTIONXY" value = "0.3" />
|
||||
<define name="OFH_COVFLOW_SETPOINT" value = "-500.f" />
|
||||
|
||||
<define name="OFH_VER_SLOPE_A" value = "3.f" />
|
||||
<define name="OFH_VER_SLOPE_B" value = "-0.5" />
|
||||
<define name="OFH_HOR_X_SLOPE_A" value = "0.003" />
|
||||
<define name="OFH_HOR_X_SLOPE_B" value = "0.005" />
|
||||
</module>
|
||||
</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="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="TYPE" value="QUAD_X"/>
|
||||
</section>
|
||||
|
||||
|
||||
<!--command_laws>
|
||||
<set servo="TOP_LEFT" value="autopilot_motors_on ? actuators_pprz[0] : -MAX_PPRZ" />
|
||||
<set servo="TOP_RIGHT" value="autopilot_motors_on ? actuators_pprz[1] : -MAX_PPRZ" />
|
||||
<set servo="BOTTOM_RIGHT" value="autopilot_motors_on ? actuators_pprz[2] : -MAX_PPRZ" />
|
||||
<set servo="BOTTOM_LEFT" value="autopilot_motors_on ? actuators_pprz[3] : -MAX_PPRZ" />
|
||||
</command_laws-->
|
||||
|
||||
<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="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"/>
|
||||
</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 -->
|
||||
<define name="HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
|
||||
<!-- For vibrating airframes -->
|
||||
<define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>
|
||||
<!-- 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"/-->
|
||||
<!-- Optitrack -->
|
||||
<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="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="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||
<!-- control effectiveness -->
|
||||
<define name="G1_P" value="0.030367"/>
|
||||
<define name="G1_Q" value="0.023655"/>
|
||||
<define name="G1_R" value="0.001005"/>
|
||||
<define name="G2_R" value="0.10882"/>
|
||||
|
||||
<!-- 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="FILT_CUTOFF_R" 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="550"/-->
|
||||
<define name="HOVER_KP" value="700"/>
|
||||
<define name="HOVER_KD" value="300"/>
|
||||
<define name="HOVER_KI" value="100"/> <!-- 13 -->
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value="0.715"/>
|
||||
<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="300"/>
|
||||
<define name="DGAIN" value="350"/>
|
||||
<define name="IGAIN" value="30"/>
|
||||
<define name="REF_MAX_SPEED" value="0.5"/>
|
||||
</section>
|
||||
|
||||
<section name="NAVIGATION" prefix="NAV_">
|
||||
<define name="CLIMB_VSPEED" value="1.0"/>
|
||||
<define name="DESCEND_VSPEED" value="-0.75"/>
|
||||
</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_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="12700"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.6" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="9.9" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="10.4" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
|
||||
</section>
|
||||
</airframe>
|
||||
@@ -0,0 +1,95 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="optical_flow_hover" dir="ctrl">
|
||||
<doc>
|
||||
<description>
|
||||
This module implements monocular vision height estimation and uses this
|
||||
estimate to stabilize a quadrotor using only a downwards
|
||||
facing camera and an IMU. For scientific background please visit:
|
||||
https://repository.tudelft.nl/islandora/object/uuid%3A6e3ce742-a974-491d-97c2-1cafc090b3d9
|
||||
|
||||
The gain height relationship as first described in
|
||||
http://iopscience.iop.org/article/10.1088/1748-3190/11/1/016004 is
|
||||
calibrated
|
||||
in a controlled environment so that the resulting relationship can be
|
||||
used to determine the height at which the quadrotor is flying.
|
||||
In addition to the vertical axis the same principle is applied to the
|
||||
horizontal axes.
|
||||
|
||||
A short guide on how to determine the slope:
|
||||
Picking a relatively high OFH_RAMP* with a very high negative
|
||||
OFH_COV*_SETPOINT (i.e. -6000000),
|
||||
looking at when the quadrotor starts oscillating, the respective message
|
||||
(covariances.*) should be used to determine a suitable
|
||||
threshold value for OFH_COV*_SETPOINT. The slope can then be decreased to gain
|
||||
accuracy in reaching the oscillations.
|
||||
Finally when flying the quadrotor on different heights the gain at which
|
||||
instability occurs can be noted to determine the slope of the
|
||||
gain-height relationship.
|
||||
|
||||
Important settings:
|
||||
-OFH_HOVER_METHOD/hover_method:
|
||||
0, the method is applied to all 3 axes in the following order: Z - X - Y
|
||||
1, the method is applied to all 3 axes at the same time
|
||||
2, the method is applied to the Z axis and the estimated height is used
|
||||
to determine the proper gains
|
||||
for the horizontal axes using the predetermined relationship.
|
||||
|
||||
-XY_SYMMETRICAL is set to 1 if the drone is roughly symmetrical,
|
||||
causing the found gains for the X axis is also used for the Y axis.
|
||||
-OFH_*GAIN* can be used to set starting gains to prevent initial drift
|
||||
while still estimating height and appropriate gains.
|
||||
-OFH_REDUCTION* is used to reduce the gain after determining the oscillation to
|
||||
stabilize the quadrotor.
|
||||
|
||||
</description>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings name="OpticalFlowHover">
|
||||
<dl_settings name="General">
|
||||
<dl_setting var="cov_method" min="0" step="1" max="1" values="input|past" module="ctrl/optical_flow_hover" shortname="COV_METHOD" param="OFH_COV_METHOD"/>
|
||||
<dl_setting var="hover_method" min="0" step="1" max="2" values="Sub|Same|Smart" module="ctrl/optical_flow_hover" shortname="HOVER_METHOD" param="OFH_HOVER_METHOD"/>
|
||||
</dl_settings>
|
||||
<dl_settings name="Vertical">
|
||||
<dl_setting var="of_hover_ctrl_Z.cov_setpoint" min="-5.05" step="0.00001" max="0" module="ctrl/optical_flow_hover" shortname="covDiv_set_point" param="OFH_COVDIV_SETPOINT"/>
|
||||
<dl_setting var="of_hover_ctrl_Z.ramp" min="0" step="0.005" max="1" module="ctrl/optical_flow_hover" shortname="rampZ" param="OFH_RAMPZ"/>
|
||||
<dl_setting var="of_hover_ctrl_Z.reduction_factor" min="0.1" step="0.01" max="1" module="ctrl/optical_flow_hover" shortname="reduction_factorZ" param="OFH_REDUCTIONZ"/>
|
||||
<dl_setting var="of_hover_ctrl_Z.PID.P" min="0" step="0.01" max="1" module="ctrl/optical_flow_hover" shortname="pgainZ"/>
|
||||
<dl_setting var="of_hover_ctrl_Z.PID.I" min="0" step="0.001" max="0.1" module="ctrl/optical_flow_hover" shortname="igainZ" param="OFH_IGAINZ"/>
|
||||
<dl_setting var="of_hover_ctrl_Z.PID.D" min="0" step="0.01" max="1" module="ctrl/optical_flow_hover" shortname="dgainZ" param="OFH_DGAINZ"/>
|
||||
</dl_settings>
|
||||
<dl_settings name="Horizontal">
|
||||
<dl_setting var="of_hover_ctrl_X.cov_setpoint" min="-100000" step="100" max="10000" module="ctrl/optical_flow_hover" shortname="covFlow_set_pointX" param="OFH_COVDIV_SETPOINT"/>
|
||||
<dl_setting var="of_hover_ctrl_Y.cov_setpoint" min="-100000" step="100" max="10000" module="ctrl/optical_flow_hover" shortname="covFlow_set_pointY" param="OFH_COVDIV_SETPOINT"/>
|
||||
<dl_setting var="of_hover_ctrl_X.ramp" min="0" step="0.0001" max="0.05" module="ctrl/optical_flow_hover" shortname="rampX" param="OFH_RAMPXY"/>
|
||||
<dl_setting var="of_hover_ctrl_Y.ramp" min="0" step="0.0001" max="0.05" module="ctrl/optical_flow_hover" shortname="rampY" param="OFH_RAMPXY"/>
|
||||
<dl_setting var="of_hover_ctrl_X.reduction_factor" min="0.1" step="0.01" max="1" module="ctrl/optical_flow_hover" shortname="reduction_factorX" param="OFH_REDUCTIONXY"/>
|
||||
<dl_setting var="of_hover_ctrl_Y.reduction_factor" min="0.1" step="0.01" max="1" module="ctrl/optical_flow_hover" shortname="reduction_factorY" param="OFH_REDUCTIONXY"/>
|
||||
<dl_setting var="of_hover_ctrl_X.PID.P" min="0" step="0.00001" max="0.01" module="ctrl/optical_flow_hover" shortname="pgainX"/>
|
||||
<dl_setting var="of_hover_ctrl_X.PID.I" min="0" step="0.00001" max="0.001" module="ctrl/optical_flow_hover" shortname="IgainX" param="OFH_IGAINX"/>
|
||||
<dl_setting var="of_hover_ctrl_X.PID.D" min="0" step="0.00001" max="0.001" module="ctrl/optical_flow_hover" shortname="DgainX" param="OFH_DGAINX"/>
|
||||
<dl_setting var="of_hover_ctrl_Y.PID.P" min="0" step="0.00001" max="0.01" module="ctrl/optical_flow_hover" shortname="pgainY"/>
|
||||
<dl_setting var="of_hover_ctrl_Y.PID.I" min="0" step="0.00001" max="0.001" module="ctrl/optical_flow_hover" shortname="IgainY" param="OFH_IGAINY"/>
|
||||
<dl_setting var="of_hover_ctrl_Y.PID.D" min="0" step="0.00001" max="0.001" module="ctrl/optical_flow_hover" shortname="DgainY" param="OFH_DGAINY"/>
|
||||
<dl_setting var="oscphi" min="0" step="1" max="1" values="No|OSC" module="ctrl/optical_flow_hover" shortname="oscphi" />
|
||||
<dl_setting var="osctheta" min="0" step="1" max="1" values="No|OSC" module="ctrl/optical_flow_hover" shortname="osctheta" />
|
||||
<dl_setting var="derotated" min="0" step="1" max="1" values="Reg|Der" module="ctrl/optical_flow_hover" shortname="derotated" />
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
|
||||
<header>
|
||||
<file name="optical_flow_hover.h" />
|
||||
<file name="optical_flow_functions.h" />
|
||||
</header>
|
||||
|
||||
<init fun="optical_flow_hover_init()" />
|
||||
|
||||
<makefile target="ap">
|
||||
<file name="optical_flow_hover.c" />
|
||||
<file name="optical_flow_functions.c" />
|
||||
</makefile>
|
||||
|
||||
</module>
|
||||
@@ -27,6 +27,7 @@
|
||||
<message name="SURVEY" period="2.5"/>
|
||||
<message name="OPTIC_FLOW_EST" period="0.05"/>
|
||||
<message name="VECTORNAV_INFO" period="0.5"/>
|
||||
<message name="OPTICAL_FLOW_HOVER" period="0.05"/>
|
||||
<message name="DIVERGENCE" period="0.05"/>
|
||||
<message name="DRAGSPEED" period="0.02"/>
|
||||
</mode>
|
||||
|
||||
@@ -1,4 +1,16 @@
|
||||
<conf>
|
||||
<aircraft
|
||||
name="ARDrone2_OF_Hover"
|
||||
ac_id="44"
|
||||
airframe="airframes/tudelft/ardrone2_OF_hover.xml"
|
||||
radio="radios/dummy.xml"
|
||||
telemetry="telemetry/default_rotorcraft.xml"
|
||||
flight_plan="flight_plans/rotorcraft_optitrack.xml"
|
||||
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_indi.xml"
|
||||
settings_modules="modules/optical_flow_hover.xml [modules/cv_opticflow.xml] modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
|
||||
gui_color="red"
|
||||
release="ac5eaa8da36b999a135f979e598930a9d40f6401"
|
||||
/>
|
||||
<aircraft
|
||||
name="ARDrone2_default"
|
||||
ac_id="10"
|
||||
@@ -130,7 +142,6 @@
|
||||
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
|
||||
settings_modules="modules/video_capture.xml modules/cv_opticflow.xml modules/air_data.xml modules/geo_mag.xml modules/ins_hff_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
|
||||
gui_color="blue"
|
||||
release=""
|
||||
/>
|
||||
<aircraft
|
||||
name="CX10"
|
||||
@@ -508,6 +519,17 @@
|
||||
gui_color="#ffffbf17bf17"
|
||||
release=""
|
||||
/>
|
||||
<aircraft
|
||||
name="bebop_OF_Hover"
|
||||
ac_id="45"
|
||||
airframe="airframes/tudelft/bebop_OF_hover.xml"
|
||||
radio="radios/dummy.xml"
|
||||
telemetry="telemetry/default_rotorcraft.xml"
|
||||
flight_plan="flight_plans/rotorcraft_optitrack.xml"
|
||||
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_indi.xml"
|
||||
settings_modules="modules/optical_flow_hover.xml modules/cv_opticflow.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
|
||||
gui_color="red"
|
||||
/>
|
||||
<aircraft
|
||||
name="bebop_flip"
|
||||
ac_id="3"
|
||||
|
||||
@@ -0,0 +1,182 @@
|
||||
/*
|
||||
* Copyright (C) 2018
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#include "optical_flow_functions.h"
|
||||
#include "paparazzi.h"
|
||||
#include "math/pprz_stat.h"
|
||||
#include "std.h"
|
||||
|
||||
// The maximum bank angle the algorithm can steer
|
||||
#ifndef OFH_MAXBANK
|
||||
#define OFH_MAXBANK 10.f
|
||||
#endif
|
||||
|
||||
// The low pass filter constant for updating the vision measurements in the algorithm
|
||||
#ifndef OF_LP_CONST
|
||||
#define OF_LP_CONST 0.5 // Average between 0.4 Bebop value and 0.6 ARDrone value
|
||||
#endif
|
||||
|
||||
// In case the autocovariance is used, select half the window size as the delay
|
||||
#ifndef OF_COV_DELAY_STEPS
|
||||
#define OF_COV_DELAY_STEPS COV_WINDOW_SIZE/2
|
||||
#endif
|
||||
|
||||
uint32_t ind_histXY;
|
||||
uint8_t cov_array_filledXY;
|
||||
uint32_t ind_histZ;
|
||||
uint8_t cov_array_filledZ;
|
||||
|
||||
|
||||
/**
|
||||
* Set the covariance of the divergence and the thrust / past divergence
|
||||
* This funciton should only be called once per time step
|
||||
* @param[in] thrust: the current thrust value
|
||||
*/
|
||||
float set_cov_div(bool cov_method, struct OFhistory *history, struct DesiredInputs *inputs)
|
||||
{
|
||||
float cov_div = 0;
|
||||
// histories and cov detection:
|
||||
|
||||
history->OF[ind_histZ] = of_hover.divergence;
|
||||
|
||||
float normalized_thrust = (float)(100.0 * inputs->thrust / MAX_PPRZ);
|
||||
history->input[ind_histZ] = normalized_thrust;
|
||||
|
||||
int ind_past = ind_histZ - OF_COV_DELAY_STEPS;
|
||||
while (ind_past < 0) { ind_past += COV_WINDOW_SIZE; }
|
||||
history->past_OF[ind_histZ] = history->OF[ind_past];
|
||||
|
||||
// determine the covariance for hover detection:
|
||||
// only take covariance into account if there are enough samples in the histories:
|
||||
if (cov_method == 0 && cov_array_filledZ > 0) {
|
||||
// TODO: step in hover set point causes an incorrectly perceived covariance
|
||||
cov_div = covariance_f(history->input, history->OF, COV_WINDOW_SIZE);
|
||||
} else if (cov_method == 1 && cov_array_filledZ > 1) {
|
||||
// todo: delay steps should be invariant to the run frequency
|
||||
cov_div = covariance_f(history->past_OF, history->OF, COV_WINDOW_SIZE);
|
||||
}
|
||||
|
||||
if (cov_array_filledZ < 2 && ind_histZ + 1 == COV_WINDOW_SIZE) {
|
||||
cov_array_filledZ++;
|
||||
}
|
||||
ind_histZ = (ind_histZ + 1) % COV_WINDOW_SIZE;
|
||||
|
||||
return cov_div;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Set the covariance of the flow and past flow / desired angle
|
||||
* This funciton should only be called once per time step
|
||||
*/
|
||||
void set_cov_flow(bool cov_method, struct OFhistory *historyX, struct OFhistory *historyY, struct DesiredInputs *inputs,
|
||||
struct FloatVect3 *covs)
|
||||
{
|
||||
// histories and cov detection:
|
||||
historyX->OF[ind_histXY] = of_hover.flowX;
|
||||
historyY->OF[ind_histXY] = of_hover.flowY;
|
||||
|
||||
int ind_past = ind_histXY - OF_COV_DELAY_STEPS;
|
||||
while (ind_past < 0) { ind_past += COV_WINDOW_SIZE; }
|
||||
historyX->past_OF[ind_histXY] = historyX->OF[ind_past];
|
||||
historyY->past_OF[ind_histXY] = historyY->OF[ind_past];
|
||||
float normalized_phi = (float)(100.0 * inputs->phi / OFH_MAXBANK);
|
||||
float normalized_theta = (float)(100.0 * inputs->theta / OFH_MAXBANK);
|
||||
historyX->input[ind_histXY] = normalized_phi;
|
||||
historyY->input[ind_histXY] = normalized_theta;
|
||||
|
||||
// determine the covariance for hover detection:
|
||||
// only take covariance into account if there are enough samples in the histories:
|
||||
if (cov_method == 0 && cov_array_filledXY > 0) {
|
||||
// // TODO: step in hover set point causes an incorrectly perceived covariance
|
||||
covs->x = covariance_f(historyX->input, historyX->OF, COV_WINDOW_SIZE);
|
||||
covs->y = covariance_f(historyY->input, historyY->OF, COV_WINDOW_SIZE);
|
||||
} else if (cov_method == 1 && cov_array_filledXY > 1) {
|
||||
if (cov_array_filledXY > 1) {
|
||||
// todo: delay steps should be invariant to the run frequency
|
||||
covs->x = covariance_f(historyX->past_OF, historyX->OF, COV_WINDOW_SIZE);
|
||||
covs->y = covariance_f(historyY->past_OF, historyY->OF, COV_WINDOW_SIZE);
|
||||
}
|
||||
}
|
||||
|
||||
if (cov_array_filledXY < 2 && ind_histXY + 1 == COV_WINDOW_SIZE) {
|
||||
cov_array_filledXY++;
|
||||
}
|
||||
ind_histXY = (ind_histXY + 1) % COV_WINDOW_SIZE;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Determine and set the desired angle for constant flow control
|
||||
* @param[out] desired angle
|
||||
* @param[in] dt: time difference since last update
|
||||
* @param[in] *of_hover_ctrl: OpticalFlowHoverControl structure
|
||||
*/
|
||||
float PID_flow_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl)
|
||||
{
|
||||
// update the controller errors:
|
||||
float lp_factor = dt / OF_LP_CONST;
|
||||
Bound(lp_factor, 0.f, 1.f);
|
||||
|
||||
// maintain the controller errors:
|
||||
of_hover_ctrl->PID.sum_err += of_hover_ctrl->PID.err;
|
||||
of_hover_ctrl->PID.d_err += (((of_hover_ctrl->PID.err - of_hover_ctrl->PID.previous_err) / dt) -
|
||||
of_hover_ctrl->PID.d_err) * lp_factor;
|
||||
of_hover_ctrl->PID.previous_err = of_hover_ctrl->PID.err;
|
||||
|
||||
// compute the desired angle
|
||||
float des_angle = of_hover_ctrl->PID.P * of_hover_ctrl->PID.err + of_hover_ctrl->PID.I *
|
||||
of_hover_ctrl->PID.sum_err + of_hover_ctrl->PID.D * of_hover_ctrl->PID.d_err;
|
||||
|
||||
// Bound angle:
|
||||
Bound(des_angle, -OFH_MAXBANK, OFH_MAXBANK);
|
||||
|
||||
return des_angle;
|
||||
}
|
||||
|
||||
/**
|
||||
* Determine and set the thrust for constant divergence control
|
||||
* @param[out] thrust
|
||||
* @param[in] dt: time difference since last update
|
||||
* @param[in] *of_hover_ctrl: OpticalFlowHoverControl structure
|
||||
*/
|
||||
int32_t PID_divergence_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl)
|
||||
{
|
||||
// update the controller errors:
|
||||
float lp_factor = dt / OF_LP_CONST;
|
||||
Bound(lp_factor, 0.f, 1.f);
|
||||
|
||||
// maintain the controller errors:
|
||||
of_hover_ctrl->PID.sum_err += of_hover_ctrl->PID.err;
|
||||
of_hover_ctrl->PID.d_err += (((of_hover_ctrl->PID.err - of_hover_ctrl->PID.previous_err) / dt) -
|
||||
of_hover_ctrl->PID.d_err) * lp_factor;
|
||||
of_hover_ctrl->PID.previous_err = of_hover_ctrl->PID.err;
|
||||
|
||||
// PID control:
|
||||
int32_t thrust = (of_hover_ctrl->nominal_value
|
||||
+ of_hover_ctrl->PID.P * of_hover_ctrl->PID.err
|
||||
+ of_hover_ctrl->PID.I * of_hover_ctrl->PID.sum_err
|
||||
+ of_hover_ctrl->PID.D * of_hover_ctrl->PID.d_err) * MAX_PPRZ;
|
||||
|
||||
// bound thrust:
|
||||
Bound(thrust, 0.25 * of_hover_ctrl->nominal_value * MAX_PPRZ, MAX_PPRZ);
|
||||
|
||||
return thrust;
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
/*
|
||||
* Copyright (C) 2018
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#ifndef OPTICAL_FLOW_FUNCTIONS_H_
|
||||
#define OPTICAL_FLOW_FUNCTIONS_H_
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
|
||||
struct GainsPID {
|
||||
float P; ///< P-gain for control
|
||||
float I; ///< I-gain for control
|
||||
float D; ///< D-gain for control
|
||||
float err; ///< Current tracking error
|
||||
float previous_err; ///< Previous tracking error
|
||||
float sum_err; ///< integration of the error for I-gain
|
||||
float d_err; ///< difference of error for the D-gain
|
||||
};
|
||||
|
||||
struct OFhistory {
|
||||
float input[COV_WINDOW_SIZE];
|
||||
float OF[COV_WINDOW_SIZE];
|
||||
float past_OF[COV_WINDOW_SIZE];
|
||||
};
|
||||
|
||||
struct OpticalFlowHoverControl {
|
||||
struct GainsPID PID; ///< The struct with the PID gains
|
||||
|
||||
float nominal_value; ///< The nominal value of thrust, phi or theta depending on Z, Y, X
|
||||
|
||||
float ramp; ///< The ramp pused is increased with per dt
|
||||
|
||||
float reduction_factor; ///< Reduce the gain by this factor when oscillating
|
||||
|
||||
float setpoint; ///< setpoint for constant divergence/flow
|
||||
float cov_setpoint; ///< for adaptive gain control, setpoint of the covariance (oscillations)
|
||||
};
|
||||
|
||||
struct OpticalFlowHover {
|
||||
float divergence; ///< Divergence estimate
|
||||
float flowX; ///< Flow estimate in X direction
|
||||
float flowY; ///< Flow estimate in Y direction
|
||||
};
|
||||
|
||||
struct DesiredInputs {
|
||||
float phi;
|
||||
float theta;
|
||||
int32_t thrust;
|
||||
};
|
||||
|
||||
extern uint32_t ind_histXY;
|
||||
extern uint8_t cov_array_filledXY;
|
||||
extern uint32_t ind_histZ;
|
||||
extern uint8_t cov_array_filledZ;
|
||||
|
||||
struct OpticalFlowHover of_hover;
|
||||
|
||||
extern float set_cov_div(bool cov_method, struct OFhistory *history, struct DesiredInputs *inputs);
|
||||
extern void set_cov_flow(bool cov_method, struct OFhistory *historyX, struct OFhistory *historyY,
|
||||
struct DesiredInputs *inputs, struct FloatVect3 *covs);
|
||||
extern float PID_flow_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl);
|
||||
extern int32_t PID_divergence_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl);
|
||||
#endif /* OPTICAL_FLOW_FUNCTIONS_H_ */
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,59 @@
|
||||
/*
|
||||
* Copyright (C) 2018
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#ifndef OPTICAL_FLOW_HOVER_H_
|
||||
#define OPTICAL_FLOW_HOVER_H_
|
||||
|
||||
#include "std.h"
|
||||
// Without optitrack set to: GUIDANCE_V/H_MODE_ATTITUDE
|
||||
// With optitrack set to: GUIDANCE_V/H_MODE_NAV
|
||||
// To use the Optical Flow Hover module use GUIDANCE_V/H_MODE_MODULE
|
||||
|
||||
#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE
|
||||
// #define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_NAV
|
||||
|
||||
#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE
|
||||
//#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_NAV
|
||||
|
||||
extern bool oscphi;
|
||||
extern bool osctheta;
|
||||
extern bool derotated;
|
||||
extern bool cov_method; ///< method to calculate the covariance: between thrust and div / angle and flow (0) or div and div past / flow and past flow(1)
|
||||
extern uint8_t hover_method; ///< Method used to hover 0 = All axis after each other; 1 = all axis at the same time; 2 = vertical only, use relation to set horizontal
|
||||
|
||||
extern struct OpticalFlowHoverControl of_hover_ctrl_X;
|
||||
extern struct OpticalFlowHoverControl of_hover_ctrl_Y;
|
||||
extern struct OpticalFlowHoverControl of_hover_ctrl_Z;
|
||||
|
||||
// The module functions
|
||||
extern void optical_flow_hover_init(void);
|
||||
|
||||
// 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);
|
||||
|
||||
// Horizontal loops
|
||||
extern void guidance_h_module_init(void);
|
||||
extern void guidance_h_module_enter(void);
|
||||
extern void guidance_h_module_run(bool in_flight);
|
||||
extern void guidance_h_module_read_rc(void);
|
||||
|
||||
#endif /* OPTICAL_FLOW_LANDING_H_ */
|
||||
Reference in New Issue
Block a user