Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 20:36:06 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[opticalflow] bebop2 with dual optical flow (#3136)

Browse Source 
* Squash on master
* fix bebeop2_flow
* Divergence is more noisy than previous time... added a low-pass filter, and allow for a higher threshold...
* fixed the sim, removed warnings, removed double airframe
* cleanup airframe, remove all warnings, allow placing camera parameters in sections
* PPRZLINK
* Silent warnings and compile errors
* reintegrated pprzlink
---------
Co-authored-by: guidoAI <guido.de.croon@gmail.com>
This commit is contained in:
Christophe De Wagter
2023-10-09 08:35:07 +02:00
committed by GitHub
parent cb97031d47
commit 4df13209e5
19 changed files with 233 additions and 507 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/examples/bebop2_opticflow.xml
+83 -125
View File
@@ -1,18 +1,28 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="bebop2_opticflow">
<description>
BEBOP2 airframe:
- requires fixed dampers
- uses bottom camera for hovering (no GPS and no Optitrack)
- uses the flow from the front camera to navigate
</description>
<firmware name="rotorcraft">
<target name="ap" board="bebop">
<define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000/log"/> <!-- we did not include log before -->
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000/log"/>
<define name="TEXTONS_DICTIONARY_PATH" value="/data/ftp/internal_000"/>
</target>
<!-- <target name="nps" board="pc">
<target name="nps" board="pc">
<module name="fdm" type="gazebo"/>
<define name="VIDEO_CAPTURE_PATH" value="/home/guido/paparazzi/images/"/>
<define name="FILE_LOGGER_PATH" value="/home/guido/paparazzi/log/"/>
<define name="VIDEO_CAPTURE_PATH" value="~/paparazzi/images/"/>
<define name="FILE_LOGGER_PATH" value="~/paparazzi/log/"/>
<define name="TEXTONS_DICTIONARY_PATH" value="~/paparazzi/images/"/>
</target>
-->
<define name="USE_SONAR"/>
<module name="telemetry" type="transparent_udp"/>
@@ -20,70 +30,31 @@
<module name="motor_mixing"/>
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="datalink"/>
<module name="gps" type="datalink"/><!-- For logging purposes -->
<module name="stabilization" type="indi_simple"/>
<!--
<module name="ins" type="flow"/>
<!-- Alternative:
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="FALSE"/>
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
<define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
</module>
<module name="ins" type="extended"/>
-->
-->
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
<define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
<define name="AHRS_FLOATING_HEADING" value="1"/>
<module name="ins" type="flow"/>
<!-- Disable use of GPS: -->
<!-- <define name="INS_INT_GPS_ID" value="ABI_DISABLE"/>
</module> -->
<!-- <module name="geo_mag"/>
<module name="air_data"/>
<module name="gps" type="ubx_ucenter"/> -->
<module name="bebop_cam">
<!-- IMPORTANT to limit these or FPS drops significantly -->
<!-- <define name="MT9F002_TARGET_FPS" value="30"/> --> <!-- Front cam -->
<define name="MT9V117_TARGET_FPS" value="90"/> <!-- Bottom cam -->
</module>
<module name="bebop_cam"/>
<module name="pose_history"/>
<module name="logger_file">
<define name="LOGGER_FILE_PATH" value="/data/ftp/internal_000"/>
<!-- <define name="LOGGER_FILE_PATH" value="/home/guido/paparazzi/log"/> -->
</module>
<module name="logger_file"/>
<module name="cv_opticflow">
<define name="OPTICFLOW_CAMERA" value="bottom_camera"/>
<define name="MAX_HORIZON" value="10"/>
<define name="OPTICFLOW_FX" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
<define name="OPTICFLOW_FY" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
<define name="OPTICFLOW_FOV_W" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
<define name="OPTICFLOW_FOV_H" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X" value="0.8"/> <!--Obtained from a linefit-->
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y" value="0.85"/> <!--Obtained from a linefit-->
<configure name="OPTICFLOW_MAX_TRACK_CORNERS" value="20"/>
<define name="OPTICFLOW_FEATURE_MANAGEMENT" value="0"/>
<define name="OPTICFLOW_FPS" value="0"/>
<define name="OPTICFLOW_PYRAMID_LEVEL" value="0"/>
<define name="OPTICFLOW_SHOW_FLOW" value="1"/>
<!--
<define name="OPTICFLOW_CAMERA2" value="front_camera"/>
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X_CAMERA2" value="0.8"/>
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y_CAMERA2" value="0.85"/>
<configure name="OPTICFLOW_MAX_TRACK_CORNERS_CAMERA2" value="20"/>
<define name="OPTICFLOW_FEATURE_MANAGEMENT_CAMERA2" value="0"/>
<define name="OPTICFLOW_FPS_CAMERA2" value="0"/>
<define name="OPTICFLOW_PYRAMID_LEVEL_CAMERA2" value="0"/>
<define name="OPTICFLOW_SHOW_FLOW_CAMERA2" value="1"/> -->
</module>
<module name="video_rtp_stream">
@@ -94,31 +65,12 @@
<define name="VIEWVIDEO_QUALITY_FACTOR" value="40"/>
</module>
<module name="video_capture">
<define name="VIDEO_CAPTURE_CAMERA" value="bottom_camera"/>
</module>
<!-- TODO: perhaps grey because of placement after optical flow? -->
<module name="cv_textons.xml">
<define name="TEXTONS_FPS" value="1"/>
<define name="TEXTONS_CAMERA" value="bottom_camera"/>
<define name="TEXTONS_N_TEXTONS" value="20"/>
<define name="TEXTONS_DICTIONARY_PATH" value="/data/ftp/internal_000"/>
</module>
<module name="optical_flow_landing.xml">
<define name="OFL_COV_SETPOINT" value="-0.075"/>
<define name="OFL_PGAIN" value="0.15"/>
<define name="OFL_IGAIN" value="0.005"/>
<define name="OFL_LP_CONST" value="0.033"/>
<define name="OFL_CLOSE_TO_EDGE" value="0.025"/>
<define name="OFL_IGAIN_ADAPTIVE" value="0.10"/>
<define name="OFL_CONTROL_METHOD" value="0"/> <!-- 1 is adaptive gain -->
<define name="OFL_P_LAND_THRESHOLD" value="0.25"/>
<define name="OFL_COV_LANDING_LIMIT" value="10.0"/>
</module>
<module name="cv_textons"/>
<module name="optical_flow_landing"/>
</firmware>
@@ -153,7 +105,14 @@
</command_laws>
<section name="BEBOP_BOTTOM_CAMERA" prefix="MT9V117_">
<!-- IMPORTANT to limit these or FPS drops significantly -->
<define name="TARGET_FPS" value="90"/>
</section>
<section name="BEBOP_FRONT_CAMERA" prefix="MT9F002_">
<!-- IMPORTANT to limit these or FPS drops significantly -->
<define name="TARGET_FPS" value="30" />
<define name="OUTPUT_HEIGHT" value="240" />
<define name="OUTPUT_WIDTH" value="240" />
<define name="OFFSET_X" value="0.14" />
@@ -161,8 +120,51 @@
<!--define name="GAIN_GREEN1" value="10.0"/>
<define name="GAIN_GREEN2" value="10.0"/>
<define name="GAIN_BLUE" value="12.5"/>
<define name="GAIN_RED" value="9.0"/-->
<define name="TARGET_EXPOSURE" value="10" />
<define name="GAIN_RED" value="9.0"/>
<define name="TARGET_EXPOSURE" value="10" /-->
</section>
<section name="OPTICAL_FLOW" prefix="OPTICFLOW_">
<define name="CAMERA" value="bottom_camera"/>
<define name="FX" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
<define name="FY" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
<define name="FOV_W" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
<define name="FOV_H" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
<define name="DEROTATION_CORRECTION_FACTOR_X" value="0.8"/> <!--Obtained from a linefit-->
<define name="DEROTATION_CORRECTION_FACTOR_Y" value="0.85"/> <!--Obtained from a linefit-->
<define name="FEATURE_MANAGEMENT" value="0"/>
<define name="FPS" value="0"/>
<define name="PYRAMID_LEVEL" value="0"/>
<define name="SHOW_FLOW" value="1"/>
<define name="CAMERA2" value="front_camera"/>
<define name="DEROTATION_CORRECTION_FACTOR_X_CAMERA2" value="0.8"/>
<define name="DEROTATION_CORRECTION_FACTOR_Y_CAMERA2" value="0.85"/>
<define name="FEATURE_MANAGEMENT_CAMERA2" value="0"/>
<define name="FPS_CAMERA2" value="0"/>
<define name="PYRAMID_LEVEL_CAMERA2" value="0"/>
<define name="SHOW_FLOW_CAMERA2" value="1"/>
</section>
<section name="OPTICAL_FLOW_LANDING" PREFIX="OFL_">
<define name="COV_SETPOINT" value="-0.075"/>
<define name="PGAIN" value="0.15"/>
<define name="IGAIN" value="0.005"/>
<define name="LP_CONST" value="0.033"/>
<define name="CLOSE_TO_EDGE" value="0.025"/>
<define name="IGAIN_ADAPTIVE" value="0.10"/>
<define name="CONTROL_METHOD" value="0"/> <!-- 1 is adaptive gain -->
<define name="P_LAND_THRESHOLD" value="0.25"/>
<define name="COV_LANDING_LIMIT" value="10.0"/>
</section>
<section name="TEXTONS" prefix="TEXTONS_">
<define name="FPS" value="1"/>
<define name="CAMERA" value="bottom_camera"/>
<define name="N_TEXTONS" value="20"/>
</section>
<section name="AIR_DATA" prefix="AIR_DATA_">
@@ -222,10 +224,12 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<!--define name="JSBSIM_MODEL" value="bebop" type="string"/-->
<define name="JSBSIM_MODEL" value="bebop" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<!-- Simulator resolution -->
<define name="MT9F002_SENSOR_RES_DIVIDER" value="4"/>
</section>
<include href="conf/simulator/gazebo/airframes/bebop.xml"/>
<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
@@ -253,8 +257,6 @@
<define name="G1_R" value="0.0075"/>
<define name="G2_R" value="0.236"/>
<!-- Here it is assumed that your removed the damping from your bebop2!
The dampers do not really damp, but cause oscillation. By removing/
fixing them, the bebop2 will fly much better-->
@@ -270,15 +272,7 @@
<define name="REF_RATE_Q" value="28.0"/>
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<!-- old?
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.7"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.7"/>
-->
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
@@ -297,12 +291,6 @@
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.52"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
<!-- old?
<define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.55"/>
<define name="ADAPT_MIN_HOVER_THROTTLE" value="0.48"/>
<define name="ADAPT_MAX_HOVER_THROTTLE" value="0.62"/>
<define name="ADAPT_NOISE_FACTOR" value="0.8"/>
-->
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
@@ -318,42 +306,11 @@
<define name="DESCEND_VSPEED" value="-0.75"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
</section>
<section name="AUTOPILOT">
<!--
<define name="MODE_STARTUP" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_NAV"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
-->
<!--
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO1" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
-->
<!--
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
-->
<define name="MODE_STARTUP" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_MANUAL" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO1" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
</section>
@@ -368,5 +325,6 @@
<define name="LOW_BAT_LEVEL" value="11.1" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
conf/airframes/tudelft/bebop2_opticflow_sim.xml
-293
View File
@@ -1,293 +0,0 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="bebop2_opticflow">
<firmware name="rotorcraft">
<target name="ap" board="bebop">
<define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000/log"/> <!-- we did not include log before -->
</target>
<target name="nps" board="pc">
<module name="fdm" type="gazebo"/>
<define name="VIDEO_CAPTURE_PATH" value="/home/guido/paparazzi/images"/>
<define name="FILE_LOGGER_PATH" value="/home/guido/paparazzi/log"/>
</target>
<define name="USE_SONAR"/>
<module name="telemetry" type="transparent_udp"/>
<module name="radio_control" type="datalink"/>
<module name="motor_mixing"/>
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="datalink"/>
<module name="stabilization" type="indi_simple"/>
<!--
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="FALSE"/>
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
<define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
</module>
<module name="ins" type="extended"/> -->
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
<define name="AHRS_FLOATING_HEADING" value="1"/>
<module name="ins" type="flow"/>
<!-- These are already defined in fdm_gazebo
<define name="NPS_BYPASS_AHRS" value="FALSE"/>
<define name="NPS_BYPASS_INS" value="FALSE"/>
-->
<!-- Disable use of GPS: -->
<!-- <define name="INS_INT_GPS_ID" value="ABI_DISABLE"/>
</module> -->
<!-- <module name="geo_mag"/>
<module name="send_imu_mag_current"/> -->
<module name="air_data"/>
<!-- <module name="gps" type="ubx_ucenter"/> -->
<module name="bebop_cam">
<!-- IMPORTANT to limit these or FPS drops significantly -->
<!-- <define name="MT9F002_TARGET_FPS" value="30"/> --> <!-- Front cam -->
<define name="MT9V117_TARGET_FPS" value="90"/> <!-- Bottom cam -->
</module>
<module name="pose_history"/>
<module name="logger_file">
<!-- <define name="FILE_LOGGER_PATH" value="/home/guido/paparazzi/log"/> -->
</module>
<module name="cv_opticflow">
<define name="OPTICFLOW_FPS" value="90"/>
<define name="OPTICFLOW_CAMERA" value="bottom_camera"/>
<define name="MAX_HORIZON" value="10"/>
<define name="OPTICFLOW_FX" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
<define name="OPTICFLOW_FY" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
<define name="OPTICFLOW_FOV_W" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
<define name="OPTICFLOW_FOV_H" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X" value="0.8"/> <!--Obtained from a linefit-->
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y" value="0.85"/> <!--Obtained from a linefit-->
</module>
<module name="video_capture">
<define name="VIDEO_CAPTURE_CAMERA" value="bottom_camera"/>
</module>
<module name="cv_textons.xml">
<define name="TEXTONS_FPS" value="1"/>
<define name="TEXTONS_CAMERA" value="bottom_camera"/>
<define name="TEXTONS_N_TEXTONS" value="20"/>
<define name="TEXTONS_DICTIONARY_PATH" value="./"/>
</module>
<module name="optical_flow_landing.xml"/>
<module name="video_rtp_stream">
<define name="VIEWVIDEO_CAMERA" value="bottom_camera"/>
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
<define name="VIEWVIDEO_QUALITY_FACTOR" value="60"/>
</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="2500" neutral="2500" max="12000"/>
<servo name="TOP_RIGHT" no="1" min="2500" neutral="2500" max="12000"/>
<servo name="BOTTOM_RIGHT" no="2" min="2500" neutral="2500" max="12000"/>
<servo name="BOTTOM_LEFT" no="3" min="2500" neutral="2500" max="12000"/>
</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="REVERSE" value="TRUE"/>
<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="AIR_DATA" prefix="AIR_DATA_">
<define name="CALC_AIRSPEED" value="FALSE"/>
<define name="CALC_TAS_FACTOR" value="FALSE"/>
<define name="CALC_AMSL_BARO" value="FALSE"/>
</section>
<!-- Magnetometer still needs to be calibrated -->
<section name="IMU" prefix="IMU_">
<define name="MAG_X_NEUTRAL" value="0"/>
<define name="MAG_Y_NEUTRAL" value="0"/>
<define name="MAG_Z_NEUTRAL" value="0"/>
<define name="MAG_X_SENS" value="7.28514789391" integer="16"/>
<define name="MAG_Y_SENS" value="7.33022132691" integer="16"/>
<define name="MAG_Z_SENS" value="7.57102035692" integer="16"/>
</section>
<!--
<section name="AHRS" prefix="AHRS_">
<define name="HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
<define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>
<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="USE_GPS_ALT" value="1"/>
<define name="VFF_R_GPS" value="0.01"/>
<define name="VFF_VZ_R_GPS" value="0.01"/>
<define name="SONAR_MIN_RANGE" value="0.0"/>
<define name="SONAR_MAX_RANGE" value="2.2"/>
<define name="SONAR_UPDATE_ON_AGL" value="FALSE"/>
</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="300" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="50"/>
</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="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="MT9F002_SENSOR_RES_DIVIDER" value="4"/>
</section>
<include href="conf/simulator/gazebo/airframes/bebop.xml"/>
<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>
<!-- SIMULATION SETTINGS -->
<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="350"/>
<define name="HOVER_KD" value="85"/>
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.52"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
<!-- old?
<define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.55"/>
<define name="ADAPT_MIN_HOVER_THROTTLE" value="0.48"/>
<define name="ADAPT_MAX_HOVER_THROTTLE" value="0.62"/>
<define name="ADAPT_NOISE_FACTOR" value="0.8"/>
-->
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="REF_MAX_SPEED" value="2" unit="m/s"/>
<define name="MAX_BANK" value="35" unit="deg"/>
<define name="PGAIN" value="82"/> <!-- GPS: 120 no GPS: 0-->
<define name="DGAIN" value="68"/> <!-- GPS: 80 no GPS: 302-->
<define name="IGAIN" value="10"/> <!-- GPS: 30 no GPS: 0-->
</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_MODULE"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<!--
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
-->
<!--
<define name="MODE_STARTUP" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_MANUAL" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_DIRECT"/>
-->
<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="11.0" unit="V"/>
<define name="LOW_BAT_LEVEL" value="11.1" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
conf/airframes/tudelft/tudelft_bebop2_opticflow_sim.xml
View File
conf/flight_plans/rotorcraft_guido_optitrack.xml
+1 -1
View File
@@ -1,6 +1,6 @@
<!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">
<flight_plan alt="3.5" ground_alt="3" lat0="51.990634" lon0="4.376789" max_dist_from_home="20" name="Rotorcraft Optitrack (Delft)" security_height="0.3">
<header>
#include "autopilot.h"
</header>
conf/flight_plans/tudelft/rotorcraft_cyberzoo_no_gps.xml
+1 -1
View File
@@ -1,6 +1,6 @@
<!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 Cyberzoo (Delft) no GPS" security_height="0.3">
<flight_plan alt="3.5" ground_alt="2" lat0="51.990634" lon0="4.376789" max_dist_from_home="20" name="Rotorcraft Cyberzoo (Delft) no GPS" security_height="0.3">
<header>
#include "autopilot.h"
</header>
conf/modules/optical_flow_landing.xml
+2
View File
@@ -31,6 +31,7 @@
<define name="IGAIN_HORIZONTAL_FACTOR" value="0.0001" description="Factor multiplied with I gain for horizontal control"/>
<define name="ROLL_TRIM" value="0.0" description="Roll trim in degrees"/>
<define name="PITCH_TRIM" value="0.0" description="Pitch trim in degrees"/>
<define name="FRONT_DIV_THRESHOLD" value="0.3" description="Threshold on front divergence for stopping."/>
<!-- TODO: add more explanations of settings below -->
</section>
</doc>
@@ -72,6 +73,7 @@
<dl_setting var="of_landing_ctrl.omega_LR" min="-60.0" step="1.0" max="60.0" module="ctrl/optical_flow_landing" shortname="omega_LR" param="OFL_OMEGA_LR"/>
<dl_setting var="of_landing_ctrl.omega_FB" min="-60.0" step="1.0" max="60.0" module="ctrl/optical_flow_landing" shortname="omega_FB" param="OFL_OMEGA_FB"/>
<dl_setting var="of_landing_ctrl.active_motion" min="0" step="1" max="2" values="none|flow|angle" module="ctrl/optical_flow_landing" shortname="active_motion" param="OFL_ACTIVE_MOTION"/>
<dl_setting var="of_landing_ctrl.front_div_threshold" min="0.0" step="0.01" max="5.0" module="ctrl/optical_flow_landing" shortname="front_div_threshold" param="OFL_FRONT_DIV_THRESHOLD"/>
</dl_settings>
</dl_settings>
</settings>
conf/userconf/tudelft/conf.xml
+3 -3
View File
@@ -161,7 +161,7 @@
telemetry="telemetry/tudelft/outback.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/heli_throttle_curve.xml modules/imu_common.xml modules/logger_sd_spi_direct.xml modules/nav_basic_rotorcraft.xml modules/opa_controller.xml modules/stabilization_int_quat.xml modules/stabilization_rate.xml modules/temp_adc.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/electrical.xml modules/geo_mag.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/heli_throttle_curve.xml modules/imu_common.xml modules/logger_sd_spi_direct.xml modules/nav_rotorcraft.xml modules/opa_controller.xml modules/stabilization_int_quat.xml modules/stabilization_rate.xml modules/temp_adc.xml"
gui_color="#ffffdffac31f"
/>
<aircraft
@@ -249,7 +249,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/dummy.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/heli_throttle_curve.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_quat.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/electrical.xml modules/geo_mag.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/heli_throttle_curve.xml modules/imu_common.xml modules/nav_rotorcraft.xml modules/stabilization_int_quat.xml"
gui_color="#ffffdffac31f"
/>
<aircraft
@@ -524,7 +524,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotorcraft_cyberzoo_no_gps.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/bebop_ae_awb.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_hff_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml modules/video_capture.xml modules/video_rtp_stream.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/bebop_ae_awb.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/electrical.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_hff_extended.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml modules/video_capture.xml modules/video_rtp_stream.xml"
gui_color="blue"
/>
<aircraft
conf/userconf/tudelft/guido_conf.xml
-11
View File
@@ -21,17 +21,6 @@
settings_modules="modules/bebop_cam.xml modules/cv_opticflow.xml modules/cv_textons.xml modules/electrical.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_flow.xml modules/nav_rotorcraft.xml modules/optical_flow_landing.xml modules/stabilization_indi_simple.xml modules/video_capture.xml modules/video_rtp_stream.xml"
gui_color="red"
/>
<aircraft
name="Bebop2_opticflow_sim"
ac_id="4"
airframe="airframes/tudelft/bebop2_opticflow_sim.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotorcraft_guido_optitrack.xml"
settings="settings/rotorcraft_basic.xml [settings/control/stabilization_indi.xml] settings/control/rotorcraft_speed.xml"
settings_modules="modules/air_data.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/cv_textons.xml modules/electrical.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_flow.xml modules/nav_rotorcraft.xml modules/optical_flow_landing.xml modules/stabilization_indi_simple.xml modules/video_capture.xml modules/video_rtp_stream.xml"
gui_color="blue"
/>
<aircraft
name="Bebop2_undistort_front"
ac_id="6"
conf/userconf/tudelft/guido_control_panel.xml
+5 -1
View File
@@ -119,7 +119,11 @@
<arg flag="-ac 9999" constant="5"/>
</program>
<program name="Messages (Python)"/>
<program name="Real-time Plotter"/>
<program name="Real-time Plotter">
<arg flag="-u" constant="0.05"/>
<arg flag="-m" constant="1000"/>
<arg flag=""/>
</program>
<program name="Messages"/>
</session>
</section>
sw/airborne/boards/bebop/mt9v117.c
-1
View File
@@ -37,7 +37,6 @@
#include <linux/videodev2.h>
#include <linux/v4l2-mediabus.h>
#include "generated/airframe.h"
#ifdef BOARD_DISCO
#include "boards/disco.h"
#else
sw/airborne/boards/bebop/mt9v117.h
+2 -1
View File
@@ -29,7 +29,8 @@
#include "std.h"
#include "mcu_periph/i2c.h"
//#include "generated/airframe.h"
// Camera parameters are defined in sections
#include "generated/airframe.h"
#ifndef MT9V117_TARGET_FPS
#define MT9V117_TARGET_FPS 0
sw/airborne/modules/computer_vision/lib/vision/edge_flow.h
+1
View File
@@ -36,6 +36,7 @@
#include "lib/vision/image.h"
#include "lib/v4l/v4l2.h"
#include "opticflow/opticflow_calculator.h"
#include "generated/airframe.h"
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
sw/airborne/modules/computer_vision/textons.c
+1
View File
@@ -34,6 +34,7 @@
#include "modules/computer_vision/cv.h"
#include "modules/computer_vision/textons.h"
#include "mcu_periph/sys_time.h"
#include "generated/airframe.h"
float ** **dictionary;
uint32_t learned_samples = 0;
sw/airborne/modules/ctrl/optical_flow_landing.c
+49 -1
View File
@@ -200,6 +200,10 @@ PRINT_CONFIG_VAR(OFL_OPTICAL_FLOW_ID)
#define OFL_ACTIVE_MOTION 0
#endif
#ifndef OFL_FRONT_DIV_THRESHOLD
#define OFL_FRONT_DIV_THRESHOLD 0.3
#endif
// Normally, horizontal control is done via sending angle commands to INDI, so 0 (false)
// When this is 1 (true),a change in angle will be commanded instead.
#define HORIZONTAL_RATE_CONTROL 0
@@ -218,11 +222,15 @@ PRINT_CONFIG_VAR(OFL_OPTICAL_FLOW_ID)
// variables retained between module calls
float old_flow_time;
// horizontal loop:
float optical_flow_x;
float optical_flow_y;
float lp_flow_x;
float lp_flow_y;
float lp_divergence_front;
float sum_roll_error;
float sum_pitch_error;
@@ -240,6 +248,8 @@ float previous_cov_err;
int32_t thrust_set;
float divergence_setpoint;
float divergence_front;
// *********************************
// include and define stuff for SSL:
// *********************************
@@ -290,7 +300,7 @@ static void send_divergence(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_DIVERGENCE(trans, dev, AC_ID,
&(of_landing_ctrl.divergence), &divergence_vision_dt, &normalized_thrust,
&cov_div, &pstate, &pused, &(of_landing_ctrl.agl));
&cov_div, &pstate, &pused, &(of_landing_ctrl.agl), &lp_divergence_front);
}
/// Function definitions
@@ -371,6 +381,8 @@ void vertical_ctrl_module_init(void)
of_landing_ctrl.omega_LR = OFL_OMEGA_LR;
of_landing_ctrl.active_motion = OFL_ACTIVE_MOTION;
of_landing_ctrl.front_div_threshold = OFL_FRONT_DIV_THRESHOLD;
int i;
if (of_landing_ctrl.use_bias) {
weights = (float *)calloc(n_textons + 1, sizeof(float));
@@ -423,6 +435,11 @@ void vertical_ctrl_module_init(void)
lp_flow_x = 0.0f;
lp_flow_y = 0.0f;
lp_divergence_front = 0.0f;
old_flow_time = get_sys_time_float();
divergence_front = 0.0f;
}
/**
@@ -484,6 +501,7 @@ static void reset_all_vars(void)
lp_flow_x = 0.0f;
lp_flow_y = 0.0f;
lp_divergence_front = 0.0f;
}
/**
@@ -899,8 +917,16 @@ void vertical_ctrl_module_run(bool in_flight)
optical_flow_x;
lp_flow_y = of_landing_ctrl.lp_factor_prediction * lp_flow_y + (1.0f - of_landing_ctrl.lp_factor_prediction) *
optical_flow_y;
lp_divergence_front = of_landing_ctrl.lp_factor_prediction * lp_divergence_front + (1.0f - of_landing_ctrl.lp_factor_prediction) *
divergence_front;
if(lp_divergence_front > of_landing_ctrl.front_div_threshold) {
// Stop moving in the longitudinal direction:
of_landing_ctrl.omega_FB = 0.0f;
}
if (of_landing_ctrl.active_motion == 1) {
// Active motion through varying ventral flow commands
float period_factor = 0.2;
@@ -1109,6 +1135,28 @@ void vertical_ctrl_optical_flow_cb(uint8_t sender_id, uint32_t stamp,
divergence_vision = size_divergence;
//printf("Reading %f, %f, %f\n", optical_flow_x, optical_flow_y, divergence_vision);
vision_time = ((float)stamp) / 1e6;
// checking fps and newness of images:
/*float new_flow_time = get_sys_time_float();
float dt = (new_flow_time - old_flow_time);
if (dt > 0) {
float fps_flow = 1.0f / dt;
printf("FPS flow bottom cam in OF landing = %f, optical_flow_x = %f\n", fps_flow, optical_flow_x);
}
old_flow_time = new_flow_time; */
}
else {
float new_flow_time = get_sys_time_float();
float dt_flow_front = new_flow_time - old_flow_time;
if (dt_flow_front > 0) {
//float fps_flow = 1.0f / dt_flow_front;
//printf("FPS flow front cam in OF landing = %f\n", fps_flow);
old_flow_time = new_flow_time;
divergence_front = size_divergence / dt_flow_front;
}
}
}
sw/airborne/modules/ctrl/optical_flow_landing.h
+1
View File
@@ -94,6 +94,7 @@ struct OpticalFlowLanding {
float omega_LR; ///< Set point for the left-right ventral flow
float omega_FB; ///< Set point for the front-back ventral flow
uint32_t active_motion; ///< Method for actively inducing motion
float front_div_threshold; ///< Threshold when the front div gets above this value, it will command a horizontal stop.
};
extern struct OpticalFlowLanding of_landing_ctrl;
sw/airborne/modules/ins/ins_flow.c
+79 -68
View File
@@ -947,21 +947,28 @@ void ins_flow_update(void)
F[i][i] = 1.0f;
}
if(CONSTANT_ALT_FILTER) {
F[OF_V_IND][OF_ANGLE_IND] = dt*(g/(cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])));
F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt*1.0f;
if(OF_TWO_DIM) {
F[OF_VX_IND][OF_THETA_IND] = dt*(g/(cosf(OF_X[OF_THETA_IND])*cosf(OF_X[OF_THETA_IND])));
if (CONSTANT_ALT_FILTER) {
F[OF_V_IND][OF_ANGLE_IND] = dt * (g / (cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND])));
F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt * 1.0f;
if (OF_TWO_DIM) {
F[OF_VX_IND][OF_THETA_IND] = dt * (g / (cosf(OF_X[OF_THETA_IND]) * cosf(OF_X[OF_THETA_IND])));
}
} else {
F[OF_V_IND][OF_ANGLE_IND] = dt * (thrust * cosf(OF_X[OF_ANGLE_IND]) / mass);
F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt * 1.0f;
F[OF_Z_IND][OF_Z_DOT_IND] = dt * 1.0f;
F[OF_Z_DOT_IND][OF_ANGLE_IND] = dt * (-thrust * sinf(OF_X[OF_ANGLE_IND]) / mass);
if (OF_THRUST_BIAS) {
F[OF_V_IND][OF_THRUST_BIAS_IND] = -dt * sinf(OF_X[OF_ANGLE_IND]) / mass;
F[OF_Z_DOT_IND][OF_THRUST_BIAS_IND] = -dt * cosf(OF_X[OF_ANGLE_IND]) / mass;
}
}
else {
F[OF_V_IND][OF_ANGLE_IND] = dt*(thrust*cosf(OF_X[OF_ANGLE_IND])/mass);
F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt*1.0f;
F[OF_Z_IND][OF_Z_DOT_IND] = dt*1.0f;
F[OF_Z_DOT_IND][OF_ANGLE_IND] = dt*(-thrust*sinf(OF_X[OF_ANGLE_IND])/mass);
if(OF_THRUST_BIAS) {
F[OF_V_IND][OF_THRUST_BIAS_IND] = -dt*sinf(OF_X[OF_ANGLE_IND]) / mass;
F[OF_Z_DOT_IND][OF_THRUST_BIAS_IND] = -dt*cosf(OF_X[OF_ANGLE_IND]) / mass;
if (OF_DRAG) {
// In MATLAB: -sign(v)*2*kd*v/m (always minus, whether v is positive or negative):
F[OF_V_IND][OF_V_IND] -= dt * 2 * kd * fabs(OF_X[OF_V_IND]) / mass;
if (OF_TWO_DIM) {
F[OF_VX_IND][OF_VX_IND] -= dt * 2 * kd * fabs(OF_X[OF_VX_IND]) / mass;
}
}
@@ -975,46 +982,49 @@ void ins_flow_update(void)
// Jacobian observation matrix H:
float H[N_MEAS_OF_KF][N_STATES_OF_KF] = {{0.}};
if(CONSTANT_ALT_FILTER) {
// Hx = [-cosf(theta)^2/z, (v*sinf(theta))/ z, (v* cosf(theta)^2)/z^2];
if (CONSTANT_ALT_FILTER) {
// Hx = [-cosf(theta)^2/z, (v*sinf(theta))/ z, (v* cosf(theta)^2)/z^2];
// lateral flow:
H[OF_LAT_FLOW_IND][OF_V_IND] = -cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/ OF_X[OF_Z_IND];
H[OF_LAT_FLOW_IND][OF_ANGLE_IND] = OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND];
H[OF_LAT_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/(OF_X[OF_Z_IND]*OF_X[OF_Z_IND]);
H[OF_LAT_FLOW_IND][OF_V_IND] = -cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
H[OF_LAT_FLOW_IND][OF_ANGLE_IND] = OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
H[OF_LAT_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) /
(OF_X[OF_Z_IND] * OF_X[OF_Z_IND]);
H[OF_LAT_FLOW_IND][OF_ANGLE_DOT_IND] = 1.0f;
// divergence:
H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]);
H[OF_DIV_FLOW_IND][OF_ANGLE_IND] = -OF_X[OF_V_IND]*cosf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND];
H[OF_DIV_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]*OF_X[OF_Z_IND]);
H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]);
H[OF_DIV_FLOW_IND][OF_ANGLE_IND] = -OF_X[OF_V_IND] * cosf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
H[OF_DIV_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND] * OF_X[OF_Z_IND]);
H[OF_DIV_FLOW_IND][OF_ANGLE_DOT_IND] = 0.0f;
if(OF_TWO_DIM) {
// divergence measurement couples the two axes actually...:
H[OF_DIV_FLOW_IND][OF_VX_IND] = -sinf(2*OF_X[OF_THETA_IND])/(2*OF_X[OF_Z_IND]);
H[OF_DIV_FLOW_IND][OF_THETA_IND] = -OF_X[OF_VX_IND]*cosf(2*OF_X[OF_THETA_IND])/OF_X[OF_Z_IND];
if (OF_TWO_DIM) {
// divergence measurement couples the two axes actually...:
H[OF_DIV_FLOW_IND][OF_VX_IND] = -sinf(2 * OF_X[OF_THETA_IND]) / (2 * OF_X[OF_Z_IND]);
H[OF_DIV_FLOW_IND][OF_THETA_IND] = -OF_X[OF_VX_IND] * cosf(2 * OF_X[OF_THETA_IND]) / OF_X[OF_Z_IND];
// lateral flow in x direction:
H[OF_LAT_FLOW_X_IND][OF_VX_IND] = -cosf(OF_X[OF_THETA_IND])*cosf(OF_X[OF_THETA_IND])/ OF_X[OF_Z_IND];
H[OF_LAT_FLOW_X_IND][OF_THETA_IND] = OF_X[OF_VX_IND]*sinf(2*OF_X[OF_THETA_IND])/OF_X[OF_Z_IND];
H[OF_LAT_FLOW_X_IND][OF_Z_IND] = OF_X[OF_VX_IND]*cosf(OF_X[OF_THETA_IND])*cosf(OF_X[OF_THETA_IND])/(OF_X[OF_Z_IND]*OF_X[OF_Z_IND]);
// lateral flow in x direction:
H[OF_LAT_FLOW_X_IND][OF_VX_IND] = -cosf(OF_X[OF_THETA_IND]) * cosf(OF_X[OF_THETA_IND]) / OF_X[OF_Z_IND];
H[OF_LAT_FLOW_X_IND][OF_THETA_IND] = OF_X[OF_VX_IND] * sinf(2 * OF_X[OF_THETA_IND]) / OF_X[OF_Z_IND];
H[OF_LAT_FLOW_X_IND][OF_Z_IND] = OF_X[OF_VX_IND] * cosf(OF_X[OF_THETA_IND]) * cosf(OF_X[OF_THETA_IND]) /
(OF_X[OF_Z_IND] * OF_X[OF_Z_IND]);
}
} else {
// lateral flow:
H[OF_LAT_FLOW_IND][OF_V_IND] = -cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/ OF_X[OF_Z_IND];
H[OF_LAT_FLOW_IND][OF_ANGLE_IND] = OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
+ OF_X[OF_Z_DOT_IND]*cosf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND];
H[OF_LAT_FLOW_IND][OF_V_IND] = -cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
H[OF_LAT_FLOW_IND][OF_ANGLE_IND] = OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]
+ OF_X[OF_Z_DOT_IND] * cosf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
H[OF_LAT_FLOW_IND][OF_ANGLE_DOT_IND] = 1.0f;
H[OF_LAT_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/(OF_X[OF_Z_IND]*OF_X[OF_Z_IND])
- OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]*OF_X[OF_Z_IND]);
H[OF_LAT_FLOW_IND][OF_Z_DOT_IND] = sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]);
H[OF_LAT_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) /
(OF_X[OF_Z_IND] * OF_X[OF_Z_IND])
- OF_X[OF_Z_DOT_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND] * OF_X[OF_Z_IND]);
H[OF_LAT_FLOW_IND][OF_Z_DOT_IND] = sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]);
// divergence:
H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]);
H[OF_DIV_FLOW_IND][OF_ANGLE_IND] = -OF_X[OF_V_IND]*cosf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
+ OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]);
H[OF_DIV_FLOW_IND][OF_ANGLE_IND] = -OF_X[OF_V_IND] * cosf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]
+ OF_X[OF_Z_DOT_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
H[OF_DIV_FLOW_IND][OF_ANGLE_DOT_IND] = 0.0f;
H[OF_DIV_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]*OF_X[OF_Z_IND])
+ OF_X[OF_Z_DOT_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/(OF_X[OF_Z_IND]*OF_X[OF_Z_IND]);
H[OF_DIV_FLOW_IND][OF_Z_DOT_IND] = -cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND];
H[OF_DIV_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND] * OF_X[OF_Z_IND])
+ OF_X[OF_Z_DOT_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / (OF_X[OF_Z_IND] * OF_X[OF_Z_IND]);
H[OF_DIV_FLOW_IND][OF_Z_DOT_IND] = -cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
}
// rate measurement:
@@ -1144,16 +1154,16 @@ void ins_flow_update(void)
// Correct the state:
// MATLAB:
// Z_expected = [-v*cosf(theta)*cosf(theta)/z + zd*sinf(2*theta)/(2*z) + thetad;
// (-v*sinf(2*theta)/(2*z)) - zd*cosf(theta)*cosf(theta)/z];
// (-v*sinf(2*theta)/(2*z)) - zd*cosf(theta)*cosf(theta)/z];
float Z_expected[N_MEAS_OF_KF];
// TODO: take this var out? It was meant for debugging...
//float Z_expect_GT_angle;
if(CONSTANT_ALT_FILTER) {
Z_expected[OF_LAT_FLOW_IND] = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
+OF_X[OF_ANGLE_DOT_IND]; // TODO: Currently, no p works better than using p here. Analyze!
if (CONSTANT_ALT_FILTER) {
Z_expected[OF_LAT_FLOW_IND] = -OF_X[OF_V_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]
+ OF_X[OF_ANGLE_DOT_IND]; // TODO: Currently, no p works better than using p here. Analyze!
/* TODO: remove later, just for debugging:
@@ -1166,10 +1176,11 @@ void ins_flow_update(void)
}
printf("\n");*/
Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]);
Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]);
if(OF_TWO_DIM) {
Z_expected[OF_LAT_FLOW_X_IND] = -OF_X[OF_VX_IND]*cosf(OF_X[OF_THETA_IND])*cosf(OF_X[OF_THETA_IND])/OF_X[OF_Z_IND]; // TODO: no q?
if (OF_TWO_DIM) {
Z_expected[OF_LAT_FLOW_X_IND] = -OF_X[OF_VX_IND] * cosf(OF_X[OF_THETA_IND]) * cosf(OF_X[OF_THETA_IND]) /
OF_X[OF_Z_IND]; // TODO: no q?
}
//Z_expect_GT_angle = -OF_X[OF_V_IND]*cosf(eulers->phi)*cosf(eulers->phi)/OF_X[OF_Z_IND];
@@ -1177,30 +1188,29 @@ void ins_flow_update(void)
if (OF_USE_GYROS) {
Z_expected[OF_RATE_IND] = OF_X[OF_ANGLE_DOT_IND]; // TODO: is this even in the right direction?
}
}
else {
Z_expected[OF_LAT_FLOW_IND] = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
+ OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND])
+ OF_X[OF_ANGLE_DOT_IND]; // TODO: We first had this rate term but not for the constant alt filter.
// Simulation and data analysis from real flights shows that including it is better. CHECK IN REALITY!
} else {
Z_expected[OF_LAT_FLOW_IND] = -OF_X[OF_V_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]
+ OF_X[OF_Z_DOT_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND])
+ OF_X[OF_ANGLE_DOT_IND]; // TODO: We first had this rate term but not for the constant alt filter.
// Simulation and data analysis from real flights shows that including it is better. CHECK IN REALITY!
Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND])
-OF_X[OF_Z_DOT_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND];
Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND])
- OF_X[OF_Z_DOT_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
//Z_expect_GT_angle = -OF_X[OF_V_IND]*cosf(eulers->phi)*cosf(eulers->phi)/OF_X[OF_Z_IND]
// + OF_X[OF_Z_DOT_IND]*sinf(2*eulers->phi)/(2*OF_X[OF_Z_IND]);
//+ OF_X[OF_ANGLE_DOT_IND];
if(N_MEAS_OF_KF == 3) {
Z_expected[OF_RATE_IND] = OF_X[OF_ANGLE_DOT_IND]; // TODO: is this even in the right direction?
// + OF_X[OF_Z_DOT_IND]*sinf(2*eulers->phi)/(2*OF_X[OF_Z_IND]);
//+ OF_X[OF_ANGLE_DOT_IND];
if (N_MEAS_OF_KF == 3) {
Z_expected[OF_RATE_IND] = OF_X[OF_ANGLE_DOT_IND]; // TODO: is this even in the right direction?
}
/*
float Z_exp_no_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
+ OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]);
float Z_exp_with_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
+ OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND])
+ OF_X[OF_ANGLE_DOT_IND];
*/
/*
float Z_exp_no_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
+ OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]);
float Z_exp_with_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
+ OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND])
+ OF_X[OF_ANGLE_DOT_IND];
*/
/*
printf("Z_exp_no_rate = %f, Z_exp_with_rate = %f, measured = %f, angle dot = %f, p = %f: ", Z_exp_no_rate, Z_exp_with_rate,
ins_flow.optical_flow_x, OF_X[OF_ANGLE_DOT_IND], dt * (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI/180.0f) / 74.0f);
@@ -1472,8 +1482,9 @@ static void ins_act_feedback_cb(uint8_t sender_id UNUSED, struct act_feedback_t
{
ins_flow.RPM_num_act = num_act;
for (int i = 0; i < num_act; i++) {
if(feedback[i].set.rpm)
if (feedback[i].set.rpm) {
ins_flow.RPM[i] = feedback[i].rpm;
}
}
}
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: cbb8ad25b1...8755be55b2
sw/tools/rtp_viewer/play_video_ffmpeg.sh
Executable
+1
View File
@@ -0,0 +1 @@
ffplay rtp_6000.sdp -protocol_whitelist file,udp,rtp -fflags nobuffer
sw/tools/rtp_viewer/rtp_viewer.py
+3
View File
@@ -17,6 +17,9 @@ os.environ['OPENCV_FFMPEG_CAPTURE_OPTIONS'] = 'protocol_whitelist;file,rtp,udp'
from pprzlink.ivy import IvyMessagesInterface
from pprzlink.message import PprzMessage
# See the issue and solution here: https://github.com/opencv/opencv/issues/10328
os.environ['OPENCV_FFMPEG_CAPTURE_OPTIONS'] = 'protocol_whitelist;file,rtp,udp'
class RtpViewer:
running = False
scale = 1