Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 15:09:25 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

Pull request with all the code for 6 DOF optical flow control, the ba… (#3028)

Browse Source 
* Pull request with all the code for 6 DOF optical flow control, the basis of a Nature and Nature Machine Intelligence paper.

* Camera ID was already added.

* Remove a duplicate airframe file.

* Cleanup

* Simulator does not work with opticflow

* Incorporated comments review

* fix provides

---------

Co-authored-by: Christophe De Wagter <dewagter@gmail.com>
This commit is contained in:
guidoAI
2023-09-13 15:46:23 +02:00
committed by GitHub
parent 10fedcc487
commit a40e0fa4e1
26 changed files with 3576 additions and 345 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/examples/bebop2_opticflow.xml
+175 -25
View File
@@ -3,8 +3,16 @@
<airframe name="bebop2_opticflow">
<firmware name="rotorcraft">
<target name="ap" board="bebop2"/>
<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"/>
@@ -14,30 +22,104 @@
<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"/>
<module name="ins" type="extended"/>
-->
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
<define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
<module name="ins" type="flow"/>
<!-- Disable use of GPS: -->
<!-- <define name="INS_INT_GPS_ID" value="ABI_DISABLE"/>
</module> -->
<module name="bebop_cam"/>
<!-- <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="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="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">
<define name="VIEWVIDEO_CAMERA" value="bottom_camera"/>
<module name="video_rtp_stream">
<define name="VIEWVIDEO_CAMERA" value="front_camera"/>
<define name="VIEWVIDEO_CAMERA2" value="bottom_camera"/>
<define name="VIEWVIDEO_FPS" value="5"/>
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
<define name="VIEWVIDEO_QUALITY_FACTOR" value="60"/>
</module> -->
<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>
</firmware>
<commands>
@@ -70,6 +152,19 @@
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
</command_laws>
<section name="BEBOP_FRONT_CAMERA" prefix="MT9F002_">
<define name="OUTPUT_HEIGHT" value="240" />
<define name="OUTPUT_WIDTH" value="240" />
<define name="OFFSET_X" value="0.14" />
<define name="ZOOM" value="3.2"/>
<!--define name="GAIN_GREEN1" value="10.0"/>
<define name="GAIN_GREEN2" value="10.0"/>
<define name="GAIN_BLUE" value="12.5"/>
<define name="GAIN_RED" value="9.0"/-->
<define name="TARGET_EXPOSURE" value="10" />
</section>
<section name="AIR_DATA" prefix="AIR_DATA_">
<define name="CALC_AIRSPEED" value="FALSE"/>
<define name="CALC_TAS_FACTOR" value="FALSE"/>
@@ -84,30 +179,36 @@
<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"/>
<define name="ACCEL_X_NEUTRAL" value="62"/>
<define name="ACCEL_Y_NEUTRAL" value="1"/>
<define name="ACCEL_Z_NEUTRAL" value="-35"/>
<define name="ACCEL_X_SENS" value="2.43379017884412" integer="16"/>
<define name="ACCEL_Y_SENS" value="2.44705938467942" integer="16"/>
<define name="ACCEL_Z_SENS" value="2.4327471718063234" integer="16"/>
</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"/>
<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="USE_GPS_ALT" value="1"/>
<!-- trust GPS a lot: -->
<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="3.5"/>
<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 -->
@@ -119,6 +220,14 @@
<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"/>
@@ -141,9 +250,18 @@
<!-- control effectiveness, can be obtained by switching on "adaptive" in the indi settings -->
<define name="G1_P" value="0.100"/>
<define name="G1_Q" value="0.050"/>
<define name="G1_R" value="0.0010"/>
<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-->
<define name="FILTER_ROLL_RATE" value="FALSE"/>
<define name="FILTER_PITCH_RATE" value="FALSE"/>
<!--define name="FILTER_YAW_RATE" value="FALSE"/-->
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="600.0"/>
<define name="REF_ERR_Q" value="600.0"/>
@@ -152,7 +270,16 @@
<define name="REF_RATE_Q" value="28.0"/>
<define name="REF_RATE_R" value="28.0"/>
<define name="FILT_CUTOFF" value="3.2"/>
<!-- 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_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
@@ -182,13 +309,13 @@
<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="DGAIN" value="102"/> <!-- 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="0.5"/>
<define name="DESCEND_VSPEED" value="-0.5"/>
<define name="CLIMB_VSPEED" value="1.0"/>
<define name="DESCEND_VSPEED" value="-0.75"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
@@ -197,20 +324,43 @@
</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_HOVER_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_HOVER_Z_HOLD"/>
<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_ATTITUDE_Z_HOLD"/>
<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>
<section name="GCS">
<define name="AC_ICON" value="quadrotor_x"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.9" unit="V"/>
conf/airframes/tudelft/bebop2_opticflow.xml → conf/airframes/tudelft/tudelft_bebop2_opticflow_sim.xml
+85 -35
View File
@@ -1,13 +1,19 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="bebop2_opticflow">
<description>?
</description>
<firmware name="rotorcraft">
<target name="ap" board="bebop2"/>
<define name="USE_SONAR"/>
<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"/>
@@ -15,46 +21,71 @@
<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="hff_extended"/>
</module>
<module name="ins" type="extended"/> -->
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
<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="logger_file">
<define name="LOGGER_FILE_PATH" value="/data/ftp/internal_000"/>
</module>
<module name="bebop_cam"/>
<module name="video_thread"/>
<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"/>
<!-- <define name="VIDEO_CAPTURE_CAMERA" value="front_camera"/> -->
<define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
</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">
<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> -->
</module>
</firmware>
<commands>
@@ -103,28 +134,28 @@
<define name="MAG_Z_SENS" value="7.57102035692" integer="16"/>
</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"/>
<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="USE_GPS_ALT" value="1"/>
<!-- trust GPS a lot: -->
<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 -->
@@ -136,6 +167,14 @@
<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"/>
@@ -154,12 +193,14 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
</section>
<!-- SIMULATION SETTINGS -->
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness, can be obtained by switching on "adaptive" in the indi settings -->
<define name="G1_P" value="0.100"/>
<define name="G1_Q" value="0.050"/>
<define name="G1_R" value="0.0010"/>
<define name="G2_R" value="0.236"/>
<!-- 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"/>
@@ -169,12 +210,17 @@
<define name="REF_RATE_Q" value="28.0"/>
<define name="REF_RATE_R" value="28.0"/>
<define name="FILT_CUTOFF" value="3.2"/>
<!--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.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<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"/>
@@ -208,17 +254,21 @@
<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_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"/>
conf/flight_plans/tudelft/rotorcraft_guido_optitrack.xml
+140
View File
@@ -0,0 +1,140 @@
<!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"
#include "modules/datalink/datalink.h"
#include "modules/energy/electrical.h"
#include "modules/radio_control/radio_control.h"
#include "modules/ahrs/ahrs.h"
#define NPS_GAZEBO_WORLD "cyberzoo_mat.world"
</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.990632" lon="4.376810" name="p1"/>
<waypoint lat="51.990622" lon="4.376786" 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>
<exceptions>
<!-- Datalink lost (constant RPM descent) -->
<exception cond="((datalink_time > 5) @AND
!(IndexOfBlock('Holding point') > nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="land here"/>
<!-- Geofencing XY -->
<!-- <exception cond="(!InsideCyberZoo(GetPosX(), GetPosY()) @AND
!(IndexOfBlock('Holding point') > nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="land here"/> -->
<!-- Geofencing Z 4.5 -->
<exception cond="((GetPosAlt() > 3.0) @AND
!(IndexOfBlock('Holding point') > nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="land here"/>
<!-- Geofencing Z 6.0 (kill throttle?)-->
<exception cond="((GetPosAlt() > 5.0) @AND
!(IndexOfBlock('Holding point') > nav_block) @AND
(autopilot_in_flight() == true) )" deroute="landed"/>
<!-- Bat low -->
<!-- <exception cond="(electrical.bat_low @AND
!(IndexOfBlock('Holding point') > nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="land here"/> -->
<!-- Bat critical (constant RPM no stabilization)-->
<exception cond="(electrical.bat_critical @AND
!(IndexOfBlock('Holding point') > nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="land here"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Holding point">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine">
<call_once fun="NavResurrect()"/>
<call_once fun="NavSetWaypointHere(WP_STDBY)"/>
<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_once 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_once 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_once 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>5" wp="p2"/>
<go from="p2" hmode="route" wp="p1"/>
<stay until="stage_time>5" wp="p1"/>
<deroute block="line_p1_p2"/>
</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_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land">
<go wp="TD"/>
<deroute block="flare"/>
</block>
<block name="flare">
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="landed"/>
<call_once fun="NavStartDetectGround()"/>
<stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
conf/modules/cv_textons.xml
+13 -6
View File
@@ -5,16 +5,21 @@
<description>Represent the appearance (texture, color) of an image by means of a texton histogram.</description>
<section name="TEXTONS" prefix="TEXTONS_">
<define name="FPS" value="0" description="The (maximum) frequency to run the calculations at. If zero, it will max out at the camera frame rate"/>
<define name="CAMERA" value="bottom_camera|front_camera" description="The V4L2 camera device that is used for the calculations"/>
<define name="RUN" value="YES" description="Whether the texton function is running (YES) or not (NO)."/>
<define name="LOAD_DICTIONARY" value="YES" description="Whether a dictionary is loaded (YES) or learned (NO)."/>
<define name="N_TEXTONS" value="20" description="The number of textons (words) in the dictionary."/>
<define name="REINITIALIZE_DICTIONARY" value="NO" description="If set to YES, the dictionary will be reinitialized with the current image."/>
<define name="N_TEXTONS" value="10" description="The number of textons (words) in the dictionary."/>
<define name="PATCH_SIZE" value="6" description="Size of the image patches extracted from the image - even numbers."/>
<define name="FULL_SAMPLING" value="NO" description="If YES, the entire image is covered with samples, if NO sub-sampling is used (faster)."/>
<define name="DICTIONARY_NUMBER" value="0" description="Number of the dictionary (so that different environments can each have their specific dictionary)."/>
<define name="N_SAMPLES" value="50" description="Number of samples extracted form the image when not doing full sampling."/>
<define name="N_LEARNING_SAMPLES" value="5000" description="Number of samples used for learning the dictionary."/>
<define name="ALPHA" value="10" description="Learning rate when creating the dictionary: 0 = no learning, 255 = the texton becomes equal to the new patch."/>
<define name="N_SAMPLES_IMAGE" value="50" description="Number of samples extracted form the image when not doing full sampling."/>
<define name="N_LEARNING_SAMPLES" value="10000" description="Number of samples used for learning the dictionary."/>
<define name="BORDER_WIDTH" value="0" description="Width of the image border from which no samples are taken."/>
<define name="BORDER_HEIGHT" value="0" description="Height of the border from which no samples are taken."/>
<define name="DICTIONARY_PATH" value="/data/ftp/internal_000" description="Path to which the textons dictionary is saved."/>
</section>
</doc>
@@ -22,14 +27,16 @@
<settings>
<dl_settings>
<dl_settings NAME="Textons">
<dl_setting var="running" min="0" step="1" max="1" module="computer_vision/textons" shortname="running" values="NO|YES" param="TEXTONS_RUN"/>
<dl_setting var="load_dictionary" min="0" step="1" max="1" module="computer_vision/textons" shortname="load dict." values="NO|YES" param="TEXTONS_LOAD_DICTIONARY"/>
<dl_setting var="reinitialize_dictionary" min="0" step="1" max="1" module="computer_vision/textons" shortname="reinit dict." values="NO|YES" param="TEXTONS_REINITIALIZE_DICTIONARY"/>
<dl_setting var="n_textons" min="1" step="1" max="255" shortname="n_textons" param="TEXTONS_N_TEXTONS"/>
<dl_setting var="patch_size" min="2" step="2" max="40" shortname="p_size" param="TEXTONS_PATCH_SIZE" />
<dl_setting var="FULL_SAMPLING" min="0" step="1" max="1" shortname="full_sam" values="NO|YES" param="TEXTONS_FULL_SAMPLING" />
<dl_setting var="dictionary_number" min="0" step="1" max="20" shortname="dict_num" param="TEXTONS_DICTIONARY_NUMBER" />
<dl_setting var="n_samples_image" min="0" step="10" max="1000" shortname="n_samples" param="TEXTONS_ALPHA" />
<dl_setting var="n_samples_image" min="0" step="10" max="1000" shortname="n_samples" param="TEXTONS_N_SAMPLES" />
<dl_setting var="n_learning_samples" min="1" step="100" max="250000" shortname="n_l_samples" param="TEXTONS_N_LEARNING_SAMPLES" />
<dl_setting var="alpha" min="0" step="1" max="255" shortname="alpha" param="TEXTONS_ALPHA"/>
<dl_setting var="alpha_uint" min="0" step="1" max="255" shortname="alpha" param="TEXTONS_ALPHA"/>
<dl_setting var="border_width" min="0" step="1" max="200" shortname="b_w" param="TEXTONS_BORDER_WIDTH"/>
<dl_setting var="border_height" min="0" step="1" max="200" shortname="b_h" param="TEXTONS_BORDER_HEIGHT"/>
</dl_settings>
@@ -41,7 +48,7 @@
</header>
<init fun="textons_init()"/>
<makefile target="ap">
<makefile target="ap|nps">
<file name="textons.c"/>
<file name="image.c" dir="modules/computer_vision/lib/vision"/>
</makefile>
conf/modules/ins_flow.xml
+47
View File
@@ -0,0 +1,47 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ins_flow" dir="ins">
<doc>
<description>
simple INS and AHRS using flow
</description>
</doc>
<settings>
<dl_settings NAME="INS">
<dl_settings name="flow">
<dl_setting var="reset_filter" min="0" step="1" max="1" values="NO|YES" module="modules/ins/ins_flow"/>
<dl_setting var="run_filter" min="0" step="1" max="1" values="NO|YES" module="modules/ins/ins_flow"/>
<dl_setting var="use_filter" min="0" step="1" max="3" values="NO|ANGLE|VELOCITY|HEIGHT" module="modules/ins/ins_flow"/>
<dl_setting var="thrust_factor" min="0.01" step="0.01" max="1.50" module="modules/ins/ins_flow"/>
</dl_settings>
</dl_settings>
</settings>
<dep>
<depends>@imu</depends>
<provides>ahrs,ins</provides>
</dep>
<header>
<file name="ins_flow.h" dir="modules/ins"/>
</header>
<init fun="ins_flow_init()"/>
<periodic fun="ins_flow_update()" autorun="TRUE"/>
<makefile target="ap|nps">
<!-- FLOW files -->
<!-- <define name="USE_AHRS"/> -->
<define name="USE_AHRS_ALIGNER"/>
<define name="INS_TYPE_H" value="modules/ins/ins_flow.h" type="string"/>
<file name="ins.c" dir="modules/ins/"/>
<file name="ins_flow.c" dir="modules/ins"/>
<file name="ahrs_int_cmpl_quat.c" dir="modules/ahrs"/>
<!-- <file name="ahrs.c" dir="subsystems"/> -->
<file name="ahrs_aligner.c" dir="modules/ahrs"/>
<!-- <file name="pprz_algebra_float.c" dir="math"/> -->
<raw>
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_flow.h\"
$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_icq
</raw>
</makefile>
</module>
conf/modules/optical_flow_landing.xml
+38 -8
View File
@@ -11,17 +11,27 @@
</description>
<define name="OFL_AGL_ID" value="ABI_BROADCAST" description="Sender id of the AGL (sonar) ABI message"/>
<section name="OFL" prefix="OFL_">
<define name="PGAIN" value="0.50" description="P gain on height error"/>
<define name="IGAIN" value="0.10" description="I gain on summed height error"/>
<define name="DGAIN" value="0.0" description="D gain on summed height error"/>
<define name="PGAIN" value="0.50" description="P gain on divergence error"/>
<define name="IGAIN" value="0.10" description="I gain on summed divergence error"/>
<define name="DGAIN" value="0.0" description="D gain on differential of divergence error"/>
<define name="VISION_METHOD" value="1" description="0 = fake vision, 1 = real vision"/>
<define name="CONTROL_METHOD" value="2" description="0 = fixed gain control, 1 = adaptive gain control, 2 = exponential control"/>
<define name="CONTROL_METHOD" value="0" description="0 = fixed gain control, 1 = adaptive gain control, 2 = exponential control, 3 = learning-based control."/>
<define name="COV_METHOD" value="0" description="0 = cov(uz, div), 1 = cov(div_past, div)"/>
<define name="COV_WINDOW_SIZE" value="30" description="Number of time steps for window size for getting the covariance over time."/>
<define name="COV_LANDING_LIMIT" value="2.2" description="Covariance where the vehicle engages final landing procedure."/>
<define name="COV_SETPOINT" value="-0.0075" description="Target Covariance for adaptive gain increment."/>
<define name="LP_CONST" value="0.75" description="Low pass filter constant for divergence input."/>
<define name="COV_SETPOINT" value="-0.10" description="Target Covariance for adaptive gain increment."/>
<define name="LP_CONST" value="0.05" description="Low pass filter constant for divergence input."/>
<define name="ELC_OSCILLATE" value="true" description="Oscillate to find optimum gain before initiating landing."/>
<define name="CLOSE_TO_EDGE" value="0.025" description="When the cov_div value gets this close to the setpoint, we consider the drone to oscillate"/>
<define name="PGAIN_ADAPTIVE" value="0.50" description="P gain on cov error"/>
<define name="IGAIN_ADAPTIVE" value="0.10" description="I gain on summed cov error"/>
<define name="DGAIN_ADAPTIVE" value="0.0" description="D gain on differential cov error"/>
<define name="P_LAND_THRESHOLD" value="0.15" description="P-value at which the drone will initiate a final landing procedure, for adaptive control"/>
<define name="PGAIN_HORIZONTAL_FACTOR" value="0.0625" description="Factor multiplied with P gain for horizontal control"/>
<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"/>
<!-- TODO: add more explanations of settings below -->
</section>
</doc>
<settings>
@@ -36,14 +46,32 @@
<dl_setting var="of_landing_ctrl.igain" min="0" step="0.001" max="1" module="ctrl/optical_flow_landing" shortname="igain" param="OFL_IGAIN"/>
<dl_setting var="of_landing_ctrl.dgain" min="0" step="0.1" max="6" module="ctrl/optical_flow_landing" shortname="dgain" param="OFL_DGAIN"/>
<dl_setting var="of_landing_ctrl.VISION_METHOD" min="0" step="1" max="1" values="Ground truth|Online" module="ctrl/optical_flow_landing" shortname="VISION_METHOD" param="OFL_VISION_METHOD"/>
<dl_setting var="of_landing_ctrl.CONTROL_METHOD" min="0" step="1" max="2" values="Simple|Adaptive|Exp" module="ctrl/optical_flow_landing" shortname="CONTROL_METHOD" param="OFL_CONTROL_METHOD"/>
<dl_setting var="of_landing_ctrl.CONTROL_METHOD" min="0" step="1" max="5" values="Simple|Adaptive|Exp|Pred|Mixed|HoverPred" module="ctrl/optical_flow_landing" shortname="CONTROL_METHOD" param="OFL_CONTROL_METHOD"/>
<dl_setting var="of_landing_ctrl.COV_METHOD" min="0" step="1" max="1" values="Div-Thrust|Div-Shift div" module="ctrl/optical_flow_landing" shortname="COV_METHOD" param="OFL_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.window_size" min="5" step="5" max="100" module="ctrl/optical_flow_landing" shortname="window_size"/>
<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_setting var="of_landing_ctrl.reduction_factor_elc" min="0.1" step="0.01" max="3.0" module="ctrl/optical_flow_landing" shortname="reduction_factor_elc"/>
<dl_setting var="of_landing_ctrl.elc_oscillate" min="0" step="1" max="1" values="FALSE|TRUE" module="ctrl/optical_flow_landing" shortname="oscillate" param="OFL_ELC_OSCILLATE"/>
<!-- TODO: check if all these new settings now have corresponding code: -->
<dl_setting var="of_landing_ctrl.learn_gains" min="0" step="1" max="1" shortname="learn_gains" module="ctrl/optical_flow_landing">
<strip_button name="Learn gains from file" icon="learn.png" value="1"/> <!-- group="cv" -->
</dl_setting>
<dl_setting var="of_landing_ctrl.close_to_edge" min="0" step="0.001" max="1.0" module="ctrl/optical_flow_landing" shortname="close_to_edge"/>
<dl_setting var="of_landing_ctrl.use_bias" min="0" step="1" max="1" shortname="use_bias" module="ctrl/optical_flow_landing"/>
<dl_setting var="of_landing_ctrl.snapshot" min="0" step="1" max="1" shortname="snapshot" module="ctrl/optical_flow_landing"/>
<dl_setting var="of_landing_ctrl.lp_factor_prediction" min="0" step="0.001" max="1" module="ctrl/optical_flow_landing" shortname="lp_factor_prediction"/>
<dl_setting var="of_landing_ctrl.p_land_threshold" min="0" step="0.01" max="2.0" module="ctrl/optical_flow_landing" shortname="p_land_threshold"/>
<dl_setting var="of_landing_ctrl.ramp_duration" min="0" step="0.01" max="2.0" module="ctrl/optical_flow_landing" shortname="ramp_duration"/>
<dl_setting var="of_landing_ctrl.pgain_horizontal_factor" min="0" step="0.001" max="1" module="ctrl/optical_flow_landing" shortname="pgain_hfact" param="OFL_PGAIN_HORIZONTAL_FACTOR"/>
<dl_setting var="of_landing_ctrl.igain_horizontal_factor" min="0" step="0.0001" max="1" module="ctrl/optical_flow_landing" shortname="igain_hfact" param="OFL_IGAIN_HORIZONTAL_FACTOR"/>
<dl_setting var="of_landing_ctrl.roll_trim" min="-60.0" step="0.1" max="60.0" module="ctrl/optical_flow_landing" shortname="roll_trim" param="OFL_ROLL_TRIM"/>
<dl_setting var="of_landing_ctrl.pitch_trim" min="-60.0" step="0.1" max="60.0" module="ctrl/optical_flow_landing" shortname="pitch_trim" param="OFL_PITCH_TRIM"/>
<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_settings>
</dl_settings>
</settings>
@@ -52,8 +80,10 @@
<file name="optical_flow_landing.h"/>
</header>
<makefile target="ap">
<makefile target="ap|nps">
<file name="optical_flow_landing.c"/>
<file name="pprz_algebra_float.c" dir="math"/>
<file name="pprz_matrix_decomp_float.c" dir="math"/>
</makefile>
</module>
conf/telemetry/highspeed_rotorcraft.xml
+1
View File
@@ -45,6 +45,7 @@
<message name="HYBRID_GUIDANCE" period="0.4"/>
<message name="ESC" period="0.5"/>
<message name="AHRS_REF_QUAT" period="0.1"/>
<message name="INS_FLOW_INFO" period="0.1"/>
<message name="GPS_RTK" period="0.1"/>
</mode>
conf/userconf/tudelft/guido_conf.xml
+17 -6
View File
@@ -7,23 +7,34 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_guido_optitrack.xml"
settings="settings/rotorcraft_basic.xml"
gui_color="#fffff996b847"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/cv_opticflow.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/optical_flow_landing.xml modules/stabilization_indi_simple.xml"
gui_color="#fffff996b847"
/>
<aircraft
name="Bebop2_opticflow"
ac_id="4"
airframe="airframes/tudelft/bebop2_opticflow.xml"
ac_id="5"
airframe="airframes/examples/bebop2_opticflow.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_guido_optitrack.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_hff_extended.xml modules/nav_basic_rotorcraft.xml modules/optical_flow_landing.xml modules/stabilization_indi_simple.xml modules/video_capture.xml"
settings_modules="modules/bebop_cam.xml modules/cv_opticflow.xml modules/cv_textons.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/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/cv_opticflow.xml [modules/cv_textons.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="5"
ac_id="6"
airframe="airframes/tudelft/bebop2_undistort_front.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
conf/userconf/tudelft/guido_control_panel.xml
+71 -190
View File
@@ -4,84 +4,85 @@
<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 name="GCS" command="sw/ground_segment/cockpit/gcs">
<arg flag="-speech"/>
<arg flag="-maximize"/>
<arg flag="-center_ac"/>
<arg flag="-mercator"/>
<arg flag="-maps_no_http"/>
<arg flag="-track_size" constant="200"/>
<arg flag="-zoom" constant="40."/>
</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"/>
<arg flag="sm600.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="NatNet3" command="sw/ground_segment/python/natnet3.x/natnet2ivy.py"/>
<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>
<program name="NatNet" command="sw/ground_segment/misc/natnet2ivy"/>
<program name="Gazebo" command="sw/tools/gzclient_launcher.sh"/>
<program name="Real-time Distance Counter" command="sw/ground_segment/python/distance_counter/dist.py"/>
</section>
<section name="sessions">
<session name="Bluegiga">
<session name="Simulation - Gazebo">
<program name="Simulator">
<arg flag="-a" constant="@AIRCRAFT"/>
<arg flag="-t" constant="nps"/>
</program>
<program name="Data Link">
<arg flag="-udp"/>
<arg flag="-udp_port" constant="4242"/>
<arg flag="-udp_uplink_port" constant="4252"/>
<arg flag="-ping_period" constant="100"/>
</program>
<program name="Server">
<arg flag="-n"/>
</program>
<program name="Server"/>
<program name="Messages"/>
<program name="GCS">
<arg flag="-speech" />
<arg flag="-speech"/>
<arg flag="-maximize"/>
<arg flag="-center_ac"/>
<arg flag="-mercator"/>
<arg flag="-maps_no_http"/>
<arg flag="-track_size" constant="200"/>
<arg flag="-zoom" constant="40."/>
</program>
<program name="NatNet3">
<arg flag="-ac" constant="1"/>
<arg flag="12"/>
<arg flag="-sm"/>
<arg flag="-f" constant="5"/>
<arg flag="-zf"/>
<arg flag="-o"/>
<program name="Gazebo"/>
</session>
<session name="Simulation - Gazebo + Joystick">
<program name="Simulator">
<arg flag="-a" constant="@AIRCRAFT"/>
<arg flag="-t" constant="nps"/>
</program>
<program name="Data Link">
<arg flag="-udp"/>
<arg flag="-ping_period" constant="100"/>
</program>
<program name="Server">
<arg flag="-n"/>
</program>
<program name="GCS">
<arg flag="-speech"/>
<arg flag="-maximize"/>
<arg flag="-center_ac"/>
<arg flag="-mercator"/>
<arg flag="-maps_no_http"/>
<arg flag="-track_size" constant="200"/>
<arg flag="-zoom" constant="40."/>
</program>
<program name="Joystick">
<arg flag="-d" constant="2"/>
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="hobbyking.xml"/>
<arg flag="sm600.xml"/>
<arg flag="-d 0"/>
</program>
<program name="Gazebo"/>
</session>
<session name="Guido">
<session name="Flight UDP">
<program name="Data Link">
<arg flag="-udp"/>
</program>
@@ -90,156 +91,36 @@
<arg flag="-speech"/>
</program>
<program name="Joystick">
<arg flag="-ac" constant="ARDrone2_optitrack"/>
<arg flag="hobbyking.xml"/>
<arg flag="-d" constant="1"/>
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="sm600.xml"/>
<arg flag="-d 0"/>
</program>
<program name="NatNet3">
<arg flag="-ac" constant="2"/>
<arg flag="11"/>
<arg flag="-zf"/>
<arg flag="-o"/>
<program name="NatNet">
<arg flag="-ac 9999" constant="@AC_ID"/>
</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 name="Distance Counter">
<program name="Real-time Distance Counter"/>
</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">
<session name="optic flow hover">
<program name="Data Link">
<arg flag="-udp"/>
</program>
<program name="Server"/>
<program name="GCS">
<arg flag="-speech" />
<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 name="Joystick">
<arg flag="-ac" constant="Bebop2_opticflow"/>
<arg flag="sm600.xml"/>
<arg flag="-d" constant="0"/>
</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 name="NatNet">
<arg flag="-ac 9999" constant="5"/>
</program>
</session>
<session name="Messages and Settings">
<program name="Messages (Python)"/>
<program name="Real-time Plotter"/>
<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>
data/pictures/gcs_icons/learn.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.6 KiB

sw/airborne/math/RANSAC.c
+6 -9
View File
@@ -55,17 +55,19 @@
*
*/
void RANSAC_linear_model(int n_samples, int n_iterations, float error_threshold, float *targets, int D,
float (*samples)[D], uint16_t count, float *params, float *fit_error __attribute__((unused)))
float (*samples)[D], uint16_t count, bool use_bias, float *params, float *fit_error __attribute__((unused)))
{
uint8_t D_1 = D + 1;
uint8_t D_1 = D;
if (use_bias) {
D_1++;
}
float err;
float errors[n_iterations];
int indices_subset[n_samples];
float subset_targets[n_samples];
float subset_samples[n_samples][D];
float subset_params[n_iterations][D_1];
bool use_bias = true;
// ensure that n_samples is high enough to ensure a result for a single fit:
n_samples = (n_samples < D_1) ? D_1 : n_samples;
@@ -88,12 +90,7 @@ void RANSAC_linear_model(int n_samples, int n_iterations, float error_threshold,
// fit a linear model on the small system:
fit_linear_model(subset_targets, D, subset_samples, n_samples, use_bias, subset_params[i], &err);
printf("params normal: %f, %f\n", subset_params[i][0], subset_params[i][1]);
float priors[2];
priors[0] = 1.0f;
priors[1] = 10.0f;
fit_linear_model_prior(subset_targets, D, subset_samples, n_samples, use_bias, priors, subset_params[i], &err);
printf("params prior: %f, %f\n", subset_params[i][0], subset_params[i][1]);
// determine the error on the whole set:
float err_sum = 0.0f;
sw/airborne/math/RANSAC.h
+2 -1
View File
@@ -50,11 +50,12 @@ extern "C" {
* @param[in] samples The samples / feature vectors
* @param[in] D The dimensionality of the samples
* @param[in] count The number of samples
* @param[in] use_bias Whether the RANSAC procedure should add a bias. If 0 it does not.
* @param[out] parameters* Parameters of the linear fit
* @param[out] fit_error* Total error of the fit
*/
void RANSAC_linear_model(int n_samples, int n_iterations, float error_threshold, float *targets, int D,
float (*samples)[D], uint16_t count, float *params, float *fit_error);
float (*samples)[D], uint16_t count, bool use_bias, float *params, float *fit_error);
/** Get indices without replacement.
*
sw/airborne/math/pprz_algebra_float.c
-1
View File
@@ -1064,4 +1064,3 @@ void float_vect2_scale_in_2d(struct FloatVect2 *vect2, float norm_des) {
vect2->y *= scale;
}
}
sw/airborne/math/pprz_algebra_float.h
+25
View File
@@ -881,6 +881,31 @@ static inline void float_mat_diagonal_scal(float **o, float v, int n)
}
}
/** Divide a matrix by a scalar */
static inline void float_mat_div_scalar(float **o, float **a, float scalar, int m, int n)
{
int i, j;
for (i = 0; i < m; i++) {
for (j = 0; j < n; j++) {
o[i][j] = a[i][j] / scalar;
}
}
}
/** Multiply a matrix by a scalar */
static inline void float_mat_mul_scalar(float **o, float **a, float scalar, int m, int n)
{
int i, j;
for (i = 0; i < m; i++) {
for (j = 0; j < n; j++) {
o[i][j] = a[i][j] * scalar;
}
}
}
extern bool float_mat_inv_2d(float inv_out[4], float mat_in[4]);
extern void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in);
extern bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4]);
sw/airborne/modules/ahrs/ahrs.h
+1
View File
@@ -43,6 +43,7 @@
#define AHRS_COMP_ID_VECTORNAV 11
#define AHRS_COMP_ID_EKF2 12
#define AHRS_COMP_ID_MADGWICK 13
#define AHRS_COMP_ID_FLOW 14
/* include actual (primary) implementation header */
#ifdef AHRS_TYPE_H
sw/airborne/modules/computer_vision/opticflow/linear_flow_fit.c
+123 -6
View File
@@ -42,12 +42,16 @@
#include "math/pprz_algebra_float.h"
#include "math/pprz_matrix_decomp_float.h"
#include "math/pprz_simple_matrix.h"
#include "math/RANSAC.h"
#include "std.h"
// Is this still necessary?
#define MAX_COUNT_PT 50
#define MIN_SAMPLES_FIT 3
#define N_PAR_TR_FIT 6
/**
* Analyze a linear flow field, retrieving information such as divergence, surface roughness, focus of expansion, etc.
* @param[out] outcome If 0, there were too few vectors for a fit. If 1, the fit was successful.
@@ -60,7 +64,8 @@
* @param[in] im_height Image height in pixels
* @param[out] info Contains all info extracted from the linear flow fit.
*/
bool analyze_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, int im_width, int im_height, struct linear_flow_fit_info *info)
bool analyze_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations,
int n_samples, int im_width, int im_height, struct linear_flow_fit_info *info)
{
// Are there enough flow vectors to perform a fit?
if (count < MIN_SAMPLES_FIT) {
@@ -70,7 +75,8 @@ bool analyze_linear_flow_field(struct flow_t *vectors, int count, float error_th
// fit linear flow field:
float parameters_u[3], parameters_v[3], min_error_u, min_error_v;
fit_linear_flow_field(vectors, count, n_iterations, error_threshold, n_samples, parameters_u, parameters_v, &info->fit_error, &min_error_u, &min_error_v, &info->n_inliers_u, &info->n_inliers_v);
fit_linear_flow_field(vectors, count, n_iterations, error_threshold, n_samples, parameters_u, parameters_v,
&info->fit_error, &min_error_u, &min_error_v, &info->n_inliers_u, &info->n_inliers_v);
// extract information from the parameters:
extract_information_from_parameters(parameters_u, parameters_v, im_width, im_height, info);
@@ -100,7 +106,9 @@ bool analyze_linear_flow_field(struct flow_t *vectors, int count, float error_th
* @param[out] n_inliers_u* Number of inliers in the horizontal flow fit.
* @param[out] n_inliers_v* Number of inliers in the vertical flow fit.
*/
void fit_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, float *parameters_u, float *parameters_v, float *fit_error, float *min_error_u, float *min_error_v, int *n_inliers_u, int *n_inliers_v)
void fit_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples,
float *parameters_u, float *parameters_v, float *fit_error, float *min_error_u, float *min_error_v, int *n_inliers_u,
int *n_inliers_v)
{
// We will solve systems of the form A x = b,
@@ -310,7 +318,8 @@ void fit_linear_flow_field(struct flow_t *vectors, int count, float error_thresh
* @param[out] info Contains all info extracted from the linear flow fit
*/
void extract_information_from_parameters(float *parameters_u, float *parameters_v, int im_width, int im_height, struct linear_flow_fit_info *info)
void extract_information_from_parameters(float *parameters_u, float *parameters_v, int im_width, int im_height,
struct linear_flow_fit_info *info)
{
// This method assumes a linear flow field in x- and y- direction according to the formulas:
// u = parameters_u[0] * x + parameters_u[1] * y + parameters_u[2]
@@ -323,8 +332,10 @@ void extract_information_from_parameters(float *parameters_u, float *parameters_
// translation orthogonal to the camera axis:
// flow in the center of the image:
info->relative_velocity_x = -(parameters_u[2] + (im_width / 2.0f) * parameters_u[0] + (im_height / 2.0f) * parameters_u[1]);
info->relative_velocity_y = -(parameters_v[2] + (im_width / 2.0f) * parameters_v[0] + (im_height / 2.0f) * parameters_v[1]);
info->relative_velocity_x = -(parameters_u[2] + (im_width / 2.0f) * parameters_u[0] +
(im_height / 2.0f) * parameters_u[1]);
info->relative_velocity_y = -(parameters_v[2] + (im_width / 2.0f) * parameters_v[0] +
(im_height / 2.0f) * parameters_v[1]);
float arv_x = fabsf(info->relative_velocity_x);
float arv_y = fabsf(info->relative_velocity_y);
@@ -372,3 +383,109 @@ void extract_information_from_parameters(float *parameters_u, float *parameters_
} else { info->focus_of_expansion_y = 0.0f; }
}
/**
* Analyze a flow field, retrieving information on relative velocities and rotations, etc.
* @param[out] outcome If 0, there were too few vectors for a fit. If 1, the fit was successful.
* @param[in] vectors The optical flow vectors
* @param[in] count The number of optical flow vectors
* @param[in] error_threshold Error used to determine inliers / outliers.
* @param[in] n_iterations Number of RANSAC iterations.
* @param[in] n_samples Number of samples used for a single fit (min. 5).
* @param[in] im_width Image width in pixels
* @param[in] im_height Image height in pixels
* @param[in] focal_length Focal length in pixels
* @param[out] info Contains all info extracted from the linear flow fit.
*/
bool analyze_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples,
int im_width, int im_height, float focal_length, struct linear_flow_fit_info *info)
{
// Are there enough flow vectors to perform a fit?
if (count < N_PAR_TR_FIT) {
// return that no fit was made:
return false;
}
return true;
/*
// fit linear flow field:
//float parameters_u[N_PAR_TR_FIT], parameters_v[N_PAR_TR_FIT], min_error_u, min_error_v;
float parameters_comb[N_PAR_TR_FIT], min_error_comb;
// normalize the flow vectors. Is supposed to be better for the fit and the parameters will be easier to interpret:
struct flow_t *normalized_vectors = (struct flow_t *) malloc(count *sizeof(struct flow_t));
//float targets_x[count];
//float targets_y[count];
// separate horizontal / vertical fit:
//float D_x[count][N_PAR_TR_FIT-1]; // bias is added in the RANSAC routine
//float D_y[count][N_PAR_TR_FIT-1];
// combined fit:
int total_count = 2*count;
float targets[total_count];
float D_comb[total_count][N_PAR_TR_FIT];
int index;
for (int i = 0; i < count; i++) {
// normalize vectors:
normalized_vectors[i].pos.x = (vectors[i].pos.x - (im_width / 2.0f)) / focal_length;
normalized_vectors[i].pos.y = (vectors[i].pos.y - (im_height / 2.0f)) / focal_length;
normalized_vectors[i].flow_x = vectors[i].flow_x / focal_length;
normalized_vectors[i].flow_y = vectors[i].flow_y / focal_length;
// Combine vertical and horizontal flow in one system:
// As in "Estimation of Visual Motion Parameters Used for Bio-inspired Navigation", by Alkowatly et al.
index = i*2;
targets[index] = normalized_vectors[i].flow_x;
D_comb[index][0] = 1.0f;
D_comb[index][1] = normalized_vectors[i].pos.x;
D_comb[index][2] = normalized_vectors[i].pos.y;
D_comb[index][3] = normalized_vectors[i].pos.x * normalized_vectors[i].pos.y;
D_comb[index][4] = normalized_vectors[i].pos.x * normalized_vectors[i].pos.x;
D_comb[index][5] = 0.0f;
index++;
targets[index] = normalized_vectors[i].flow_y;
D_comb[index][0] = 0.0f;
D_comb[index][1] = normalized_vectors[i].pos.y;
D_comb[index][1] = -normalized_vectors[i].pos.x;
D_comb[index][2] = normalized_vectors[i].pos.y * normalized_vectors[i].pos.y;
D_comb[index][3] = normalized_vectors[i].pos.x * normalized_vectors[i].pos.y;
D_comb[index][4] = 1.0f;
}
// Perform RANSAC on the horizontal flow:
// RANSAC_linear_model(n_samples, n_iterations, error_threshold, targets_x, N_PAR_TR_FIT, D_x, count, parameters_u, &min_error_u);
// Perform RANSAC on the combined system:
bool use_bias = false;
RANSAC_linear_model(n_samples, n_iterations, error_threshold, targets, N_PAR_TR_FIT+1, D_comb, total_count, parameters_comb, &min_error_comb, use_bias);
// extract information from the parameters:
info->rotation_X = parameters_comb[3];
info->rotation_Y = -parameters_comb[4];
info->rotation_Z = parameters_comb[2];
info->divergence = parameters_comb[1];
info->relative_velocity_x = -parameters_comb[0] - info->rotation_Y;
info->relative_velocity_y = -parameters_comb[5] + info->rotation_X;
if(fabs(parameters_comb[1]) > 1E-3) {
info->focus_of_expansion_x = info->relative_velocity_x / parameters_comb[1];
info->focus_of_expansion_y = info->relative_velocity_y / parameters_comb[1];
}
else {
// FoE in the center of the image:
info->focus_of_expansion_x = 0.0f;
info->focus_of_expansion_y = 0.0f;
}
// free up allocated memory:
free(normalized_vectors);
// return successful fit:
return true;
*/
}
sw/airborne/modules/computer_vision/opticflow/linear_flow_fit.h
+14 -3
View File
@@ -51,16 +51,27 @@ struct linear_flow_fit_info {
float fit_error; ///< Error of the fit (same as surface roughness)
int n_inliers_u; ///< Number of inliers in the horizontal flow fit
int n_inliers_v; ///< Number of inliers in the vertical flow fit
float rotation_X; ///< Rotation around the X axis
float rotation_Y; ///< Rotation around the Y axis
float rotation_Z; ///< Rotation around the Z axis
};
// This is the function called externally, passing the vector of optical flow vectors and information on the number of vectors and image size:
bool analyze_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, int im_width, int im_height, struct linear_flow_fit_info *info);
bool analyze_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations,
int n_samples, int im_width, int im_height, struct linear_flow_fit_info *info);
// Fits the linear flow field with RANSAC:
void fit_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, float *parameters_u, float *parameters_v, float *fit_error, float *min_error_u, float *min_error_v, int *n_inliers_u, int *n_inliers_v);
void fit_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples,
float *parameters_u, float *parameters_v, float *fit_error, float *min_error_u, float *min_error_v, int *n_inliers_u,
int *n_inliers_v);
// Extracts relevant information from the fit parameters:
void extract_information_from_parameters(float *parameters_u, float *parameters_v, int im_width, int im_height, struct linear_flow_fit_info *info);
void extract_information_from_parameters(float *parameters_u, float *parameters_v, int im_width, int im_height,
struct linear_flow_fit_info *info);
// Analyze the optic flow field, also determining the rotations:
bool analyze_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples,
int im_width, int im_height, float focal_length, struct linear_flow_fit_info *info);
#endif
sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
+16 -5
View File
@@ -63,7 +63,7 @@ uint16_t n_agents[2] = {25, 25};
#define SIZE_DIV 1
// LINEAR_FIT makes a linear optical flow field fit and extracts a lot of information:
// relative velocities in x, y, z (divergence / time to contact), the slope of the surface, and the surface roughness.
#define LINEAR_FIT 1
#define LINEAR_FIT 0
#ifndef OPTICFLOW_CORNER_METHOD
#define OPTICFLOW_CORNER_METHOD ACT_FAST
@@ -477,7 +477,8 @@ void opticflow_calc_init(struct opticflow_t opticflow[])
opticflow[1].id = 1;
struct FloatEulers euler_cam2 = {OPTICFLOW_BODY_TO_CAM_PHI_CAMERA2, OPTICFLOW_BODY_TO_CAM_THETA_CAMERA2,
OPTICFLOW_BODY_TO_CAM_PSI_CAMERA2};
OPTICFLOW_BODY_TO_CAM_PSI_CAMERA2
};
float_rmat_of_eulers(&body_to_cam[1], &euler_cam2);
#endif
}
@@ -682,6 +683,14 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img,
result->divergence = fit_info.divergence;
result->surface_roughness = fit_info.surface_roughness;
// Flow fit with rotations:
error_threshold = 10.0f;
n_iterations_RANSAC = 20;
n_samples_RANSAC = 5;
success_fit = analyze_flow_field(vectors, result->tracked_cnt, error_threshold, n_iterations_RANSAC,
n_samples_RANSAC, img->w, img->h, OPTICFLOW_CAMERA.camera_intrinsics.focal_x, &fit_info);
} else {
result->divergence = 0.0f;
result->surface_roughness = 0.0f;
@@ -728,14 +737,15 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img,
} else {
// determine the roll, pitch, yaw differencces between the images.
// determine the roll, pitch, yaw differences between the images.
float phi_diff = opticflow->img_gray.eulers.phi - opticflow->prev_img_gray.eulers.phi;
float theta_diff = opticflow->img_gray.eulers.theta - opticflow->prev_img_gray.eulers.theta;
float psi_diff = opticflow->img_gray.eulers.psi - opticflow->prev_img_gray.eulers.psi;
if (strcmp(opticflow->camera->dev_name, bottom_camera.dev_name) == 0) {
// bottom cam: just subtract a scaled version of the roll and pitch difference from the global flow vector:
diff_flow_x = phi_diff * opticflow->camera->camera_intrinsics.focal_x; // phi_diff works better than (cam_state->rates.p)
diff_flow_x = phi_diff *
opticflow->camera->camera_intrinsics.focal_x; // phi_diff works better than (cam_state->rates.p)
diff_flow_y = theta_diff * opticflow->camera->camera_intrinsics.focal_y;
result->flow_der_x = result->flow_x - diff_flow_x * opticflow->subpixel_factor *
opticflow->derotation_correction_factor_x;
@@ -1113,7 +1123,8 @@ bool calc_edgeflow_tot(struct opticflow_t *opticflow, struct image_t *img,
result->tracked_cnt = getAmountPeaks(edge_hist_x, 500, img->w);
result->divergence = -1.0 * (float)edgeflow.div_x /
RES; // Also multiply the divergence with -1.0 to make it on par with the LK algorithm of
result->div_size = result->divergence; // Fill the div_size with the divergence to atleast get some divergenge measurement when switching from LK to EF
result->div_size =
result->divergence; // Fill the div_size with the divergence to atleast get some divergenge measurement when switching from LK to EF
result->camera_id = opticflow->id;
result->surface_roughness = 0.0f;
//......................Calculating VELOCITY ..................... //
sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
+8 -5
View File
@@ -79,6 +79,8 @@ struct opticflow_t {
int actfast_min_gradient; ///< Threshold that decides when there is sufficient texture for edge following
int actfast_gradient_method; ///< Whether to use a simple or Sobel filter
uint16_t fps;
const struct video_config_t *camera;
uint8_t id;
};
@@ -87,15 +89,16 @@ struct opticflow_t {
extern void opticflow_calc_init(struct opticflow_t opticflow[]);
extern bool opticflow_calc_frame(struct opticflow_t *opticflow, struct image_t *img,
struct opticflow_result_t *result);
struct opticflow_result_t *result);
extern bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img,
struct opticflow_result_t *result);
struct opticflow_result_t *result);
extern bool calc_edgeflow_tot(struct opticflow_t *opticflow, struct image_t *img,
struct opticflow_result_t *result);
struct opticflow_result_t *result);
extern void kalman_filter_opticflow_velocity(float *velocity_x, float *velocity_y, float *acceleration_measurement, float fps,
float *measurement_noise, float process_noise, bool reinitialize_kalman);
extern void kalman_filter_opticflow_velocity(float *velocity_x, float *velocity_y, float *acceleration_measurement,
float fps,
float *measurement_noise, float process_noise, bool reinitialize_kalman);
#endif /* OPTICFLOW_CALCULATOR_H */
sw/airborne/modules/computer_vision/opticflow_module.c
+8 -4
View File
@@ -40,6 +40,8 @@
#include "cv.h"
uint16_t fps_OF;
/* ABI messages sender ID */
#ifndef OPTICFLOW_AGL_ID
#define OPTICFLOW_AGL_ID ABI_BROADCAST ///< Default sonar/agl to use in opticflow visual_estimator
@@ -70,7 +72,8 @@ static bool opticflow_got_result[ACTIVE_CAMERAS]; ///< When we have an opt
static pthread_mutex_t opticflow_mutex; ///< Mutex lock fo thread safety
/* Static functions */
struct image_t *opticflow_module_calc(struct image_t *img, uint8_t camera_id); ///< The main optical flow calculation thread
struct image_t *opticflow_module_calc(struct image_t *img,
uint8_t camera_id); ///< The main optical flow calculation thread
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
@@ -149,7 +152,7 @@ void opticflow_module_run(void)
opticflow_result[idx_camera].noise_measurement,
opticflow_result[idx_camera].noise_measurement,
-1.0f //opticflow_result.noise_measurement // negative value disables filter updates with OF-based vertical velocity.
);
);
}
opticflow_got_result[idx_camera] = false;
}
@@ -174,8 +177,9 @@ struct image_t *opticflow_module_calc(struct image_t *img, uint8_t camera_id)
img->eulers = pose.eulers;
// Do the optical flow calculation
static struct opticflow_result_t temp_result[ACTIVE_CAMERAS]; // static so that the number of corners is kept between frames
if(opticflow_calc_frame(&opticflow[camera_id], img, &temp_result[camera_id])){
static struct opticflow_result_t
temp_result[ACTIVE_CAMERAS]; // static so that the number of corners is kept between frames
if (opticflow_calc_frame(&opticflow[camera_id], img, &temp_result[camera_id])) {
// Copy the result if finished
pthread_mutex_lock(&opticflow_mutex);
opticflow_result[camera_id] = temp_result[camera_id];
sw/airborne/modules/computer_vision/textons.c
+98 -21
View File
@@ -33,20 +33,38 @@
#include <stdio.h>
#include "modules/computer_vision/cv.h"
#include "modules/computer_vision/textons.h"
#include "mcu_periph/sys_time.h"
float ** **dictionary;
uint32_t learned_samples = 0;
uint8_t dictionary_initialized = 0;
float *texton_distribution;
#define MAX_N_TEXTONS 255
// initial settings:
#ifndef TEXTONS_RUN
#define TEXTONS_RUN 1
#endif
PRINT_CONFIG_VAR(TEXTONS_RUN)
#ifndef TEXTONS_FPS
#define TEXTONS_FPS 30
#endif
PRINT_CONFIG_VAR(TEXTONS_FPS)
#ifndef TEXTONS_LOAD_DICTIONARY
#define TEXTONS_LOAD_DICTIONARY 1
#endif
PRINT_CONFIG_VAR(TEXTONS_LOAD_DICTIONARY)
#ifndef TEXTONS_REINITIALIZE_DICTIONARY
#define TEXTONS_REINITIALIZE_DICTIONARY 0
#endif
PRINT_CONFIG_VAR(TEXTONS_REINITIALIZE_DICTIONARY)
#ifndef TEXTONS_ALPHA
#define TEXTONS_ALPHA 10
#define TEXTONS_ALPHA 0
#endif
PRINT_CONFIG_VAR(TEXTONS_ALPHA)
@@ -56,7 +74,7 @@ PRINT_CONFIG_VAR(TEXTONS_ALPHA)
PRINT_CONFIG_VAR(TEXTONS_N_TEXTONS)
#ifndef TEXTONS_N_SAMPLES
#define TEXTONS_N_SAMPLES 100
#define TEXTONS_N_SAMPLES 250
#endif
PRINT_CONFIG_VAR(TEXTONS_N_SAMPLES)
@@ -66,7 +84,7 @@ PRINT_CONFIG_VAR(TEXTONS_N_SAMPLES)
PRINT_CONFIG_VAR(TEXTONS_PATCH_SIZE)
#ifndef TEXTONS_N_LEARNING_SAMPLES
#define TEXTONS_N_LEARNING_SAMPLES 10000
#define TEXTONS_N_LEARNING_SAMPLES 5000
#endif
PRINT_CONFIG_VAR(TEXTONS_N_LEARNING_SAMPLES)
@@ -90,8 +108,15 @@ PRINT_CONFIG_VAR(TEXTONS_BORDER_HEIGHT)
#endif
PRINT_CONFIG_VAR(TEXTONS_DICTIONARY_NUMBER)
#ifndef TEXTONS_DICTIONARY_PATH
#define TEXTONS_DICTIONARY_PATH /data/ftp/internal_000
#endif
struct video_listener *listener = NULL;
uint8_t running = TEXTONS_RUN;
uint8_t load_dictionary = TEXTONS_LOAD_DICTIONARY;
uint8_t reinitialize_dictionary = TEXTONS_REINITIALIZE_DICTIONARY;
uint8_t alpha_uint = TEXTONS_ALPHA;
uint8_t n_textons = TEXTONS_N_TEXTONS;
uint8_t patch_size = TEXTONS_PATCH_SIZE;
@@ -108,18 +133,17 @@ float alpha = 0.0;
// File pointer for saving the dictionary
static FILE *dictionary_logger = NULL;
#ifndef DICTIONARY_PATH
#define DICTIONARY_PATH /data/video/
#endif
/**
* Main texton processing function that first either loads or learns a dictionary and then extracts the texton histogram.
* @param[out] *img The output image
* @param[in] *img The input image (YUV422)
*/
struct image_t *texton_func(struct image_t *img);
struct image_t *texton_func(struct image_t *img)
struct image_t *texton_func(struct image_t *img, uint8_t p);
struct image_t *texton_func(struct image_t *img, uint8_t p)
{
// whether to execute the function:
if (!running) { return img; }
if (img->buf_size == 0) { return img; }
@@ -129,9 +153,24 @@ struct image_t *texton_func(struct image_t *img)
// if patch size odd, correct:
if (patch_size % 2 == 1) { patch_size++; }
// check whether we have to reinitialize the dictionary:
if (reinitialize_dictionary) {
// set all vars to trigger a reinitialization and learning phase of the dictionary:
dictionary_ready = 0;
dictionary_initialized = 0;
load_dictionary = 0;
learned_samples = 0;
alpha_uint = 10;
// reset reinitialize_dictionary
reinitialize_dictionary = 0;
}
// if dictionary not initialized:
if (dictionary_ready == 0) {
if (load_dictionary == 0) {
printf("Learned samples: %d / %d\n", learned_samples, n_learning_samples);
// Train the dictionary:
DictionaryTrainingYUV(frame, img->w, img->h);
@@ -143,14 +182,35 @@ struct image_t *texton_func(struct image_t *img)
dictionary_ready = 1;
// lower learning rate
alpha = 0.0;
printf("Enough learning!\n");
alpha_uint = 0;
// set learned samples back to 0
learned_samples = 0;
}
} else {
// Load the dictionary:
load_texton_dictionary();
}
} else {
// Extract distributions
DistributionExtraction(frame, img->w, img->h);
if (alpha_uint > 0) {
// printf("Learning, frame time = %d\n", img->ts.tv_sec * 1000 + img->ts.tv_usec / 1000);
DictionaryTrainingYUV(frame, img->w, img->h);
if (learned_samples >= n_learning_samples) {
// Save the dictionary:
save_texton_dictionary();
// reset learned_samples:
learned_samples = 0;
}
} else {
// Extract distributions
DistributionExtraction(frame, img->w, img->h);
}
// printf("N textons = %d\n", n_samples_image);
// printf("Entropy texton distribution = %f\n", get_entropy(texton_distribution, n_textons));
}
return img; // Colorfilter did not make a new image
@@ -320,8 +380,6 @@ void DistributionExtraction(uint8_t *frame, uint16_t width, uint16_t height)
// EXECUTION
// ************************
printf("Execute!\n");
// Allocate memory for texton distances and image patch:
float *texton_distances, * **patch;
texton_distances = (float *)calloc(n_textons, sizeof(float));
@@ -411,9 +469,10 @@ void DistributionExtraction(uint8_t *frame, uint16_t width, uint16_t height)
}
// Normalize distribution:
for (i = 0; i < n_textons; i++) {
texton_distribution[i] = texton_distribution[i] / (float) n_extracted_textons;
// printf("textons[%d] = %f\n", i, texton_distribution[i]);
if (n_extracted_textons > 0) { // should always be the case
for (i = 0; i < n_textons; i++) {
texton_distribution[i] = texton_distribution[i] / (float) n_extracted_textons;
}
}
// printf("\n");
@@ -444,11 +503,12 @@ void save_texton_dictionary(void)
char filename[512];
// Check for available files
sprintf(filename, "%s/Dictionary_%05d.dat", STRINGIFY(DICTIONARY_PATH), dictionary_number);
sprintf(filename, "%s/Dictionary_%05d.dat", STRINGIFY(TEXTONS_DICTIONARY_PATH), dictionary_number);
dictionary_logger = fopen(filename, "w");
if (dictionary_logger == NULL) {
printf("Filename: %s\n", filename);
perror("Error while opening the file.\n");
} else {
// (over-)write dictionary
@@ -471,7 +531,7 @@ void save_texton_dictionary(void)
void load_texton_dictionary(void)
{
char filename[512];
sprintf(filename, "%s/Dictionary_%05d.dat", STRINGIFY(DICTIONARY_PATH), dictionary_number);
sprintf(filename, "%s/Dictionary_%05d.dat", STRINGIFY(TEXTONS_DICTIONARY_PATH), dictionary_number);
if ((dictionary_logger = fopen(filename, "r"))) {
// Load the dictionary:
@@ -501,12 +561,12 @@ void load_texton_dictionary(void)
void textons_init(void)
{
printf("Textons init\n");
texton_distribution = (float *)calloc(n_textons, sizeof(float));
texton_distribution = (float *)calloc(MAX_N_TEXTONS, sizeof(float));
dictionary_initialized = 0;
learned_samples = 0;
dictionary_ready = 0;
dictionary = (float ** **)calloc(n_textons, sizeof(float ** *));
for (int w = 0; w < n_textons; w++) {
dictionary = (float ** **)calloc(MAX_N_TEXTONS, sizeof(float ** *));
for (int w = 0; w < MAX_N_TEXTONS; w++) {
dictionary[w] = (float ** *) calloc(patch_size, sizeof(float **));
for (int i = 0; i < patch_size; i++) {
dictionary[w][i] = (float **) calloc(patch_size, sizeof(float *));
@@ -516,7 +576,7 @@ void textons_init(void)
}
}
cv_add(texton_func);
listener = cv_add_to_device(&TEXTONS_CAMERA, texton_func, TEXTONS_FPS, 0);
}
void textons_stop(void)
@@ -525,3 +585,20 @@ void textons_stop(void)
free(dictionary);
}
/**
* Function that calculates a base-2 Shannon entropy for a probability distribution.
* @param[in] p_dist The probability distribution array
* @param[in] D Size of the array
*/
float get_entropy(float *p_dist, int D)
{
float entropy = 0.0f;
int i;
for (i = 0; i < D; i++) {
if (p_dist[i] > 0) {
entropy -= p_dist[i] * log2(p_dist[i]);
}
}
return entropy;
}
sw/airborne/modules/computer_vision/textons.h
+5
View File
@@ -39,7 +39,9 @@
extern float *texton_distribution; // main outcome of the image processing: the distribution of textons in the image
// settings
extern uint8_t running;
extern uint8_t load_dictionary;
extern uint8_t reinitialize_dictionary;
extern uint8_t alpha_uint;
extern uint8_t n_textons;
extern uint8_t patch_size; // Should be even
@@ -67,4 +69,7 @@ void load_texton_dictionary(void);
extern void textons_init(void);
extern void textons_stop(void);
// helper functions (potentially should go elsewhere):
float get_entropy(float *p_dist, int D);
#endif /* TEXTONS_H */
sw/airborne/modules/ctrl/optical_flow_landing.c
+899 -16
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/ctrl/optical_flow_landing.h
+44 -4
View File
@@ -46,10 +46,13 @@
#include "std.h"
// maximum number of samples to learn from for SSL:
#define MAX_SAMPLES_LEARNING 25000
struct OpticalFlowLanding {
float agl; ///< agl = height from sonar (only used when using "fake" divergence)
float agl_lp; ///< low-pass version of agl
float lp_const; ///< low-pass filter constant
float lp_const; ///< low-pass value for divergence
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)
@@ -61,7 +64,7 @@ struct OpticalFlowLanding {
float d_err; ///< difference of error for the D-gain
float nominal_thrust; ///< nominal thrust around which the PID-control operates
uint32_t VISION_METHOD; ///< whether to use vision (1) or Optitrack / sonar (0)
uint32_t CONTROL_METHOD; ///< type of divergence control: 0 = fixed gain, 1 = adaptive gain
uint32_t CONTROL_METHOD; ///< type of divergence control: 0 = fixed gain, 1 = adaptive gain, 2 = exponential time landing control. 3 = learning control
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
@@ -75,20 +78,57 @@ struct OpticalFlowLanding {
float t_transition; ///< how many seconds the drone has to be oscillating in order to transition to the hover phase with reduced gain
float p_land_threshold; ///< if during the exponential landing the gain reaches this value, the final landing procedure is triggered
bool elc_oscillate; ///< Whether or not to oscillate at beginning of elc to find optimum gain
// Added features for SSL:
volatile bool
learn_gains; ///< set to true if the robot needs to learn a mapping from texton distributions to the p-gain
bool load_weights; ///< load the weights that were learned before
float close_to_edge; ///< if abs(cov_div - reference cov_div) < close_to_edge, then texton distributions and gains are stored for learning
bool use_bias; ///< if true, a bias 1.0f will be added to the feature vector (texton distribution) - this typically leads to very large weights
bool snapshot; ///< if true, besides storing a texton distribution, an image will also be stored (when unstable)
float lp_factor_prediction; ///< low-pass value for the predicted P-value
float ramp_duration; ///< ramp duration in seconds for when the divergence setpoint changes
float pgain_horizontal_factor;///< factor multiplied with the vertical P-gain for horizontal ventral-flow-based control
float igain_horizontal_factor;///< factor multiplied with the vertical I-gain for horizontal ventral-flow-based control
float roll_trim; ///< Roll trim angle in degrees
float pitch_trim; ///< Pitch trim angle in degrees
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
};
extern struct OpticalFlowLanding of_landing_ctrl;
// Without optitrack set to: GUIDANCE_H_MODE_ATTITUDE
// With optitrack set to: GUIDANCE_H_MODE_HOVER / NAV
#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_NAV
// With optitrack set to: GUIDANCE_H_MODE_HOVER / NAV (NAV is the common option in the experiments.)
#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE
// Own guidance_v
#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE
// Implement own horizontal loop:
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);
// 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);
// SSL functions:
void save_texton_distribution(void);
void load_texton_distribution(void);
void fit_linear_model_OF(float *targets, float **samples, uint8_t D, uint16_t count, float *parameters,
float *fit_error);
void learn_from_file(void);
float predict_gain(float *distribution);
void save_weights(void);
void load_weights(void);
void recursive_least_squares_batch(float *targets, float **samples, uint8_t D, uint16_t count, float *params,
float *fit_error);
void recursive_least_squares(float target, float *sample, uint8_t length_sample, float *params);
#endif /* OPTICAL_FLOW_LANDING_H_ */
sw/airborne/modules/ins/ins_flow.c
+1571
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/ins/ins_flow.h
+169
View File
@@ -0,0 +1,169 @@
/*
* Copyright (C) 2021 Guido de Croon <g.c.h.e.decroon@tudelft.nl>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/ins/ins_flow.h
*
* "Inertial" navigation system.
*/
#ifndef INS_FLOW_H
#define INS_FLOW_H
#ifdef __cplusplus
extern "C" {
#endif
#define CONSTANT_ALT_FILTER 1
#define OF_DRAG 1
// Only for constant alt for now!
#define OF_TWO_DIM 1
// Only for changing alt
#define OF_THRUST_BIAS 0
// Whether to use gyros:
#define OF_USE_GYROS 1
// Predicting gyros with roll command and optic flow (normally 0):
#define PREDICT_GYROS 0
#if CONSTANT_ALT_FILTER == 1
#if OF_TWO_DIM == 0
#define OF_V_IND 0
#define OF_ANGLE_IND 1
#define OF_Z_IND 2
#define OF_ANGLE_DOT_IND 3
#if OF_USE_GYROS == 1
#define N_MEAS_OF_KF 3
#else
// gyros not used at all
#define N_MEAS_OF_KF 2
#endif
#define OF_THETA_IND -1
#define OF_VX_IND -1
#if OF_THRUST_BIAS == 0
#define N_STATES_OF_KF 4
#define OF_THRUST_BIAS_IND -1
#else
// does this work with thrust bias?
#define N_STATES_OF_KF 5
#define OF_THRUST_BIAS_IND 4
#endif
#else
#define N_STATES_OF_KF 6
#define OF_V_IND 0
#define OF_ANGLE_IND 1
#define OF_Z_IND 2
#define OF_THETA_IND 3
#define OF_VX_IND 4
#define OF_ANGLE_DOT_IND 5
// TODO: also a theta dot ind?
#define OF_THRUST_BIAS_IND -1
// the third measurement here is the other lateral flow:
#if OF_USE_GYROS == 1
// gyros used in the prediction and measurement
#define N_MEAS_OF_KF 4
#else
// gyros not used at all
#define N_MEAS_OF_KF 3
#endif
#endif
#define OF_Z_DOT_IND -1
#else
#if OF_THRUST_BIAS == 0
#define N_STATES_OF_KF 5
#define OF_THRUST_BIAS_IND -1
#else
#define N_STATES_OF_KF 6
#define OF_THRUST_BIAS_IND 5
#endif
#define OF_V_IND 0
#define OF_ANGLE_IND 1
#define OF_ANGLE_DOT_IND 2
#define OF_Z_IND 3
#define OF_Z_DOT_IND 4
#define OF_THETA_IND -1
#define OF_VX_IND -1
#if OF_USE_GYROS == 1
// gyros used in the prediction and measurement
#define N_MEAS_OF_KF 3
#else
// gyros not used at all
#define N_MEAS_OF_KF 2
#endif
#endif
// TODO: make these parameters in the estimation scheme:
#define OF_TB_Q 0.02
#define OF_TB_P 0.5
#define OF_LAT_FLOW_IND 0
#define OF_DIV_FLOW_IND 1
#define OF_RATE_IND 2
#if OF_USE_GYROS == 1
#define OF_LAT_FLOW_X_IND 3
#else
#define OF_LAT_FLOW_X_IND 2
#endif
// use filter to different extents:
#define USE_ANGLE 1
#define USE_VELOCITY 2
#define USE_HEIGHT 3
#include "modules/ahrs/ahrs.h"
#include "modules/ahrs/ahrs_int_cmpl_quat.h"
#include "modules/ahrs/ahrs_aligner.h"
#include "modules/ins/ins.h"
extern void ins_flow_init(void);
extern void ins_flow_update(void);
extern float OF_X[N_STATES_OF_KF];
extern bool reset_filter;
extern bool run_filter;
extern int use_filter;
extern float thrust_factor;
extern float GT_phi;
extern float GT_theta;
#ifdef __cplusplus
}
#endif
#endif /* INS_FLOW_H */