Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 03:57:45 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

EKF2 Optical Flow Interface (#2779)

Browse Source 
Co-authored-by: Dennis van Wijngaarden <wijngaarden.dennis@gmail.com>
This commit is contained in:
Pietro Campolucci
2021-09-15 22:26:28 +02:00
committed by GitHub
parent 3172453e74
commit 69c55a0663
17 changed files with 789 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.vscode/settings.json
Vendored
+6
View File
@@ -8,4 +8,10 @@
"[python]": { "[python]": {
"editor.tabSize": 4, "editor.tabSize": 4,
}, },
"files.associations": {
"mateksys_3901_l0x.h": "c",
"stdlib.h": "c",
"iterator": "cpp",
"tensor": "cpp"
},
} }
conf/airframes/tudelft/nederquad.xml
+309
View File
@@ -0,0 +1,309 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!-- this is a quadrotor frame equiped with
* Autopilot: Pixhawk 4
* IMU: L3GD20 + LSM303D + MPU6000 + external HMC58XX
* Actuators: PWM motor controllers
* GPS: Ublox http://wiki.paparazziuav.org/wiki/Subsystem/gps
* RC: SBUS
-->
<airframe name="nederquad">
<description> Detouchable Quadcopter for TUDelft Outback Challenge </description>
<firmware name="rotorcraft">
<target name="ap" board="px4fmu_5.0_chibios" />
<define name="BAT_CHECKER_DELAY" value="80" />
<define name="CATASTROPHIC_BATTERY_KILL_DELAY" value="80" />
<configure name="PERIODIC_FREQUENCY" value="500"/>
<module name="telemetry" type="transparent">
<configure name="MODEM_PORT" value="UART2"/>
<configure name="MODEM_BAUD" value="B115200"/>
</module>
<module name="baro_ms5611_spi" >
<configure name="MS5611_SPI_DEV" value="spi1"/>
<configure name="MS5611_SLAVE_IDX" value="SPI_SLAVE3"/>
</module>
<module name="optical_flow_mateksys_3901_l0x.xml">
<configure name="MATEKSYS_3901_L0X_PORT" value="UART3"/>
<configure name="USE_MATEKSYS_3901_L0X_AGL" value="0"/>
<!-- Calibration done live 17/08/2021 -->
<define name="MATEKSYS_3901_L0X_FLOW_X_SCALER" value="-1.13"/>
<define name="MATEKSYS_3901_L0X_FLOW_Y_SCALER" value="1.26"/>
</module>
<module name="lidar_lite">
<configure name="LIDAR_LITE_I2C_DEV" value="I2C4"/>
</module>
<!-- Logger -->
<module name="tlsf"/>
<module name="pprzlog"/>
<module name="logger" type="sd_chibios"/>
<module name="flight_recorder"/>
<module name="imu" type="mpu6000">
<define name="IMU_MPU_CHAN_X" value="1"/>
<define name="IMU_MPU_CHAN_Y" value="2"/>
<define name="IMU_MPU_CHAN_Z" value="0"/>
<define name="IMU_MPU_X_SIGN" value="+1"/>
<define name="IMU_MPU_Y_SIGN" value="-1"/>
<define name="IMU_MPU_Z_SIGN" value="-1"/>
</module>
<!-- <module name="gps" type="ublox" >
<configure name="GPS_BAUD" value="B57600"/>
</module>
<module name="gps" type="ubx_ucenter" /> -->
<module name="gps" type="datalink" >
<configure name="GPS_BAUD" value="B57600"/>
</module>
<module name="stabilization" type="indi_simple" />
<module name="ins" type="ekf2"/>
<module name="current_sensor">
<configure name="ADC_CURRENT_SENSOR" value="ADC_2" />
</module>
<module name="radio_control" type="sbus">
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
<configure name="SBUS_PORT" value="UART4"/>
</module>
<module name="geo_mag" />
<module name="air_data" />
<module name="send_imu_mag_current" />
<module name="mag" type="hmc58xx">
<define name="HMC58XX_STARTUP_DELAY" value="1.4"/>
<configure name="MAG_HMC58XX_I2C_DEV" value="i2c1"/>
<define name="MODULE_HMC58XX_SYNC_SEND" value="TRUE"/>
<define name="MODULE_HMC58XX_UPDATE_AHRS" value="TRUE"/>
<define name="HMC58XX_CHAN_X" value="1"/>
<define name="HMC58XX_CHAN_Y" value="0"/>
<define name="HMC58XX_CHAN_Z" value="2"/>
<define name="HMC58XX_CHAN_X_SIGN" value="-"/>
<define name="HMC58XX_CHAN_Y_SIGN" value="+"/>
<define name="HMC58XX_CHAN_Z_SIGN" value="+"/>
</module>
<module name="motor_mixing"/>
<module name="actuators" type="pwm">
<define name="SERVO_HZ" value="400" />
</module>
</firmware>
<section name="MISC">
<define name="BAT_CHECKER_DELAY" value="80" />
<define name="INS_EKF2_FUSION_MODE" value="MASK_USE_GPS | MASK_USE_OF"/>
<define name="INS_EKF2_VDIST_SENSOR_TYPE" value="VDIST_SENSOR_RANGE"/>
<define name="INS_EKF2_GPS_CHECK_MASK" value="0"/>
<define name="INS_EKF2_GPS_COURSE_YAW" value="FALSE"/>
</section>
<section name="IMU" prefix="IMU_">
<!-- Calibration ACCEL done 17/08/2021 -->
<define name="ACCEL_X_NEUTRAL" value="-162"/>
<define name="ACCEL_Y_NEUTRAL" value="9"/>
<define name="ACCEL_Z_NEUTRAL" value="-23"/>
<define name="ACCEL_X_SENS" value="4.781593308240347" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.841708853357494" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.897351780600174" integer="16"/>
<!-- Calibration done on ground 17/08/2021 -->
<define name="BODY_TO_IMU_PHI" value="1.1437" unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="40.6374" unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
<!-- Calibration done outside 17/08/2021 -->
<define name="MAG_X_NEUTRAL" value="188"/>
<define name="MAG_Y_NEUTRAL" value="-4"/>
<define name="MAG_Z_NEUTRAL" value="-40"/>
<define name="MAG_X_SENS" value="3.8465362336375035" integer="16"/>
<define name="MAG_Y_SENS" value="3.5338639944542787" integer="16"/>
<define name="MAG_Z_SENS" value="3.663567611662951" integer="16"/>
<!-- Calibration done props off 17/08/2021 -->
<define name= "MAG_X_CURRENT_COEF" value="0.327866157321"/>
<define name= "MAG_Y_CURRENT_COEF" value="-2.84306050424"/>
<define name= "MAG_Z_CURRENT_COEF" value="0.491914992028"/>
</section>
<commands>
<axis name="PITCH" failsafe_value="0" />
<axis name="ROLL" failsafe_value="0" />
<axis name="YAW" failsafe_value="0" />
<axis name="THRUST" failsafe_value="0" />
<axis name="RELEASE" failsafe_value="-9600" />
</commands>
<rc_commands>
<set command="THRUST" value="@THROTTLE" />
<set command="ROLL" value="@ROLL" />
<set command="PITCH" value="@PITCH" />
<set command="YAW" value="@YAW" />
<set command="RELEASE" value="@AUX2" />
</rc_commands>
<servos driver="Default">
<servo name="TOP_LEFT" no="2" min="1000" neutral="1100" max="2000" />
<servo name="TOP_RIGHT" no="3" min="1000" neutral="1100" max="2000" />
<servo name="BOTTOM_RIGHT" no="1" min="1000" neutral="1100" max="2000" />
<servo name="BOTTOM_LEFT" no="0" min="1000" neutral="1100" max="2000" />
<servo name="RELEASE_SERVO" no="4" min="1100" neutral="1400" max="2000"/>
</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]" />
<set servo="RELEASE_SERVO" value="@RELEASE" />
</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="TRUE" />
</section>
<section name="AHRS" prefix="AHRS_">
<define name="H_X" value="0.3892503" />
<define name="H_Y" value="0.0017972" />
<define name="H_Z" value="0.9211303" />
</section>
<section name="INS" prefix="INS_">
<define name="SONAR_UPDATE_ON_AGL" value="TRUE" />
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness -->
<define name="G1_P" value="0.01965369" />
<define name="G1_Q" value="0.018093345" />
<define name="G1_R" value="0.0008" />
<define name="G2_R" value="0.0000000" />
<!-- For the bebop2 we need to filter the roll rate due to the dampers -->
<define name="FILTER_ROLL_RATE" value="TRUE" />
<define name="FILTER_PITCH_RATE" value="TRUE" />
<define name="FILTER_YAW_RATE" value="TRUE" />
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="150" />
<define name="REF_ERR_Q" value="150" />
<define name="REF_ERR_R" value="49" />
<define name="REF_RATE_P" value="22.5" />
<define name="REF_RATE_Q" value="22.5" />
<define name="REF_RATE_R" value="4" />
<!-- second order filter parameters -->
<define name="FILT_CUTOFF" value="1.5"/>
<define name="FILT_CUTOFF_R" value="1.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.0354" />
<define name="ACT_DYN_Q" value="0.0354" />
<define name="ACT_DYN_R" value="0.0354" />
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE" />
<define name="ADAPTIVE_MU" value="0.0001" />
<!-- max yaw rate (conservative) -->
<define name="MAX_R" value="360" unit="deg/s"/> <!--Does not seem to be applied-->
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="25" unit="deg"/>
<define name="SP_MAX_THETA" value="25" unit="deg"/>
<define name="SP_MAX_R" value="180" unit="deg/s"/>
<define name="DEADBAND_A" value="2"/>
<define name="DEADBAND_E" value="2"/>
<define name="DEADBAND_R" value="50"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9"/>
<define name="REF_MAX_P" value="600." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9"/>
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="200" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="300." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(4000.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="600"/>
<define name="PHI_DGAIN" value="200"/>
<define name="PHI_IGAIN" value="40"/>
<define name="THETA_PGAIN" value="750"/>
<define name="THETA_DGAIN" value="250"/>
<define name="THETA_IGAIN" value="40"/>
<define name="PSI_PGAIN" value="1500"/>
<define name="PSI_DGAIN" value="600"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="0"/>
<define name="THETA_DDGAIN" value="0"/>
<define name="PSI_DDGAIN" value="100"/>
</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.50" />
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE" />
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="20" unit="deg" />
<define name="REF_MAX_SPEED" value="2" unit="m/s" />
<!-- Correct gains for perfect hovering -->
<define name="PGAIN" value="90" />
<define name="DGAIN" value="200" />
<define name="IGAIN" value="80" />
</section>
<section name="NAVIGATION" prefix="NAV_">
<define name="CLIMB_VSPEED" value="4.5" />
<define name="DESCEND_VSPEED" value="-1.0" />
</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="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.2" unit="V" />
<define name="CRITIC_BAT_LEVEL" value="9.9" unit="V" />
<define name="LOW_BAT_LEVEL" value="10.2" unit="V" />
<define name="MAX_BAT_LEVEL" value="12.4" unit="V" />
</section>
</airframe>
conf/modules/gps_datalink.xml
+1
View File
@@ -6,6 +6,7 @@
Remote GPS via datalink. Remote GPS via datalink.
Parses the REMOTE_GPS and REMOTE_GPS_SMALL datalink messages and publishes it onboard via ABI. Parses the REMOTE_GPS and REMOTE_GPS_SMALL datalink messages and publishes it onboard via ABI.
</description> </description>
<define name="GPS_DATALINK_USE_MAG" value="1" description="Choose to receive also GPS extracted magnetometer messages"/>
</doc> </doc>
<dep> <dep>
<depends>gps,@datalink</depends> <depends>gps,@datalink</depends>
conf/modules/ins_ekf2.xml
+17 -5
View File
@@ -5,11 +5,21 @@
<description> <description>
simple INS and AHRS using EKF2 from PX4 simple INS and AHRS using EKF2 from PX4
</description> </description>
<define name="INS_EKF2_SONAR_MIN_RANGE" value="0.05" description="AGL sensor minimum range [m]"/>
<define name="INS_EKF2_SONAR_MAX_RANGE" value="3" description="AGL sensor maximum range [m]"/>
<define name="INS_EKF2_RANGE_MAIN_AGL" value="1" description="If enabled uses radar sensor as primary AGL source, if possible"/>
<define name="INS_EKF2_FLOW_SENSOR_DELAY" value="15" description="flow/radar message delay [ms]"/>
<define name="INS_EKF2_MIN_FLOW_QUALITY" value="100" description="Minimum quality of the optical flow message accepted [0-255]"/>
<define name="INS_EKF2_MAX_FLOW_RATE" value="20" description="Maximum flow rate the sensor can perceive [rad/sec]]"/>
<define name="INS_EKF2_FLOW_OFFSET_X" value="-130" description="Flow sensor X offset from IMU position [mm]"/>
<define name="INS_EKF2_FLOW_OFFSET_Y" value="-150" description="Flow sensor Y offset from IMU position [mm]"/>
<define name="INS_EKF2_FLOW_OFFSET_Z" value="170" description="Flow sensor Z offset from IMU position [mm]"/>
<define name="INS_EKF2_FLOW_NOISE" value="0.01" description="Flow sensor noise [rad/sec]"/>
<define name="INS_EKF2_FLOW_NOISE_QMIN" value="0.03" description="Flow sensor noise when quality is minimum [rad/sec]"/>
<define name="INS_EKF2_FLOW_INNOV_GATE" value="3" description="Flow sensor innovation gate [STD]"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS)" description="The sensor that are used for position fusion"/> <define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS)" description="The sensor that are used for position fusion"/>
<define name="INS_EKF2_VDIST_SENSOR_TYPE" value="VDIST_SENSOR_BARO" description="Primary sensor used for vertical distance"/> <define name="INS_EKF2_VDIST_SENSOR_TYPE" value="VDIST_SENSOR_BARO" description="Primary sensor used for vertical distance"/>
<define name="INS_EKF2_GPS_CHECK_MASK" value="(MASK_GPS_NSATS | MASK_GPS_HACC | MASK_GPS_SACC)" description="GPS checks enabled before initialization of the global positioning"/> <define name="INS_EKF2_GPS_CHECK_MASK" value="(MASK_GPS_NSATS | MASK_GPS_HACC | MASK_GPS_SACC)" description="GPS checks enabled before initialization of the global positioning"/>
<define name="INS_SONAR_MIN_RANGE" value="0.001" description="Minimum range in meters for the AGL sensor"/>
<define name="INS_SONAR_MAX_RANGE" value="4.0" description="Maximum range in meters for the AGL sensor"/>
<define name="INS_EKF2_AGL_ID" value="ABI_BROADCAST" description="ABI sensor ID used as input for AGL measurements"/> <define name="INS_EKF2_AGL_ID" value="ABI_BROADCAST" description="ABI sensor ID used as input for AGL measurements"/>
<define name="INS_EKF2_BARO_ID" value="ABI_BROADCAST" description="ABI sensor ID used ad input for Barometric measurements"/> <define name="INS_EKF2_BARO_ID" value="ABI_BROADCAST" description="ABI sensor ID used ad input for Barometric measurements"/>
<define name="INS_EKF2_GYRO_ID" value="ABI_BROADCAST" description="ABI sensor ID used as input for gyro measurements"/> <define name="INS_EKF2_GYRO_ID" value="ABI_BROADCAST" description="ABI sensor ID used as input for gyro measurements"/>
@@ -22,6 +32,7 @@
<!-- EKF2 Configuration parameters --> <!-- EKF2 Configuration parameters -->
<dl_settings name="ekf2"> <dl_settings name="ekf2">
<dl_setting var="ekf2_params.mag_fusion_type" min="0" step="1" max="5" shortname="mag_fusion" values="AUTO|HEADING|3D|AUTOFW|INDOOR|NONE" module="subsystems/ins/ins_ekf2" handler="change_param"/> <dl_setting var="ekf2_params.mag_fusion_type" min="0" step="1" max="5" shortname="mag_fusion" values="AUTO|HEADING|3D|AUTOFW|INDOOR|NONE" module="subsystems/ins/ins_ekf2" handler="change_param"/>
<dl_setting var="ekf2_params.fusion_mode" min="0" max="1" step="1" shortname="remove_gps" values="FALSE|TRUE" module="subsystems/ins/ins_ekf2" handler="remove_gps" type="bool" persistent="true"/>
</dl_settings> </dl_settings>
</dl_settings> </dl_settings>
</settings> </settings>
@@ -45,9 +56,9 @@
<!-- Include the ecl and matrix libraries from ext --> <!-- Include the ecl and matrix libraries from ext -->
<include name="$(PAPARAZZI_SRC)/sw/ext/ecl/"/> <include name="$(PAPARAZZI_SRC)/sw/ext/ecl/"/>
<include name="$(PAPARAZZI_SRC)/sw/ext/matrix/"/> <include name="$(PAPARAZZI_SRC)/sw/ext/matrix/"/>
<define name="__PAPARAZZI" value="true"/> <define name="__PAPARAZZI" value="TRUE"/>
<define name="ECL_STANDALONE" value="true"/> <define name="ECL_STANDALONE" value="TRUE"/>
<define name="USE_MAGNETOMETER" value="true"/> <!-- Needed for IMU to get scaled version --> <define name="USE_MAGNETOMETER" value="TRUE"/> <!-- Needed for IMU to get scaled version -->
<!-- Compile needed ecl files --> <!-- Compile needed ecl files -->
<file name="mathlib.cpp" dir="ecl/mathlib"/> <file name="mathlib.cpp" dir="ecl/mathlib"/>
@@ -67,5 +78,6 @@
<file name="terrain_estimator.cpp" dir="ecl/EKF"/> <file name="terrain_estimator.cpp" dir="ecl/EKF"/>
<file name="vel_pos_fusion.cpp" dir="ecl/EKF"/> <file name="vel_pos_fusion.cpp" dir="ecl/EKF"/>
<file name="gps_yaw_fusion.cpp" dir="ecl/EKF"/> <file name="gps_yaw_fusion.cpp" dir="ecl/EKF"/>
</makefile> </makefile>
</module> </module>
conf/modules/optical_flow_mateksys_3901_l0x.xml
+13 -8
View File
@@ -12,11 +12,24 @@
<configure name="MATEKSYS_3901_L0X_BAUD" value="115200" description="Sets the baudrate of the UART"/> <configure name="MATEKSYS_3901_L0X_BAUD" value="115200" description="Sets the baudrate of the UART"/>
<define name="MATEKSYS_3901_L0X_MOTION_THRES" value="120" description="Sets the minimum motion quality to accept the flow measurement [0-255]"/> <define name="MATEKSYS_3901_L0X_MOTION_THRES" value="120" description="Sets the minimum motion quality to accept the flow measurement [0-255]"/>
<define name="MATEKSYS_3901_L0X_DISTANCE_THRES" value="200" description="Sets the minimum distance quality to accept the distance measurement [0-255]"/> <define name="MATEKSYS_3901_L0X_DISTANCE_THRES" value="200" description="Sets the minimum distance quality to accept the distance measurement [0-255]"/>
<define name="MATEKSYS_3901_L0X_MAX_DISTANCE" value="2500" description="Maximum allowable distance for rangefinder [mm]"/>
<define name="MATEKSYS_3901_L0X_MAX_FLOW" value="150" description="Maximum allowable flow rate measurable [deg/sec] "/>
<define name="USE_MATEKSYS_3901_L0X_AGL" value="0" description="Send AGL measurements on ABI bus"/> <define name="USE_MATEKSYS_3901_L0X_AGL" value="0" description="Send AGL measurements on ABI bus"/>
<define name="USE_MATEKSYS_3901_L0X_OPTICAL_FLOW" value="0" description="Send optical flow measurements on ABI bus"/> <define name="USE_MATEKSYS_3901_L0X_OPTICAL_FLOW" value="0" description="Send optical flow measurements on ABI bus"/>
<define name="MATEKSYS_3901_L0X_COMPENSATE_ROTATION" value="0" description="Adjust distance from ground by compensating body rotation"/> <define name="MATEKSYS_3901_L0X_COMPENSATE_ROTATION" value="0" description="Adjust distance from ground by compensating body rotation"/>
<define name="MATEKSYS_3901_L0X_FLOW_X_SCALER" value="1" description="Calibration scaling factor for flow in X direction (pitch)"/>
<define name="MATEKSYS_3901_L0X_FLOW_Y_SCALER" value="1" description="Calibration scaling factor for flow in Y direction (roll)"/>
</doc> </doc>
<settings>
<dl_settings>
<dl_settings NAME="optical_flow">
<dl_setting MAX="2" MIN="-2" STEP="0.05" VAR="mateksys3901l0x.scaler_x" shortname="scaler X" module="modules/optical_flow/mateksys_3901_l0x" param="MATEKSYS_3901_L0X_FLOW_X_SCALER" handler="scale_X" type="float" persistent="true"/>
<dl_setting MAX="2" MIN="-2" STEP="0.05" VAR="mateksys3901l0x.scaler_y" shortname="scaler Y" module="modules/optical_flow/mateksys_3901_l0x" param="MATEKSYS_3901_L0X_FLOW_Y_SCALER" handler="scale_Y" type="float" persistent="true"/>
</dl_settings>
</dl_settings>
</settings>
<header> <header>
<file name="mateksys_3901_l0x.h"/> <file name="mateksys_3901_l0x.h"/>
</header> </header>
@@ -32,18 +45,10 @@
<!-- Enable UART and set baudrate --> <!-- Enable UART and set baudrate -->
<define name="USE_$(MATEKSYS_3901_L0X_PORT_UPPER)"/> <define name="USE_$(MATEKSYS_3901_L0X_PORT_UPPER)"/>
<!-- If the driver is used on a serial port where the TX pin is not available, since the TX is not needed for this sensor it can be disabled -->
<define name="USE_$(MATEKSYS_3901_L0X_PORT)_TX" value="FALSE"/> <define name="USE_$(MATEKSYS_3901_L0X_PORT)_TX" value="FALSE"/>
<define name="$(MATEKSYS_3901_L0X_PORT_UPPER)_BAUD" value="$(MATEKSYS_3901_L0X_BAUD)"/> <define name="$(MATEKSYS_3901_L0X_PORT_UPPER)_BAUD" value="$(MATEKSYS_3901_L0X_BAUD)"/>
<define name="MATEKSYS_3901_L0X_PORT" value="$(MATEKSYS_3901_L0X_PORT_LOWER)"/> <define name="MATEKSYS_3901_L0X_PORT" value="$(MATEKSYS_3901_L0X_PORT_LOWER)"/>
<!-- Data processing configuration parameters -->
<define name="MATEKSYS_3901_L0X_MOTION_THRES" value="120"/>
<define name="MATEKSYS_3901_L0X_DISTANCE_THRES" value="200"/>
<define name="USE_MATEKSYS_3901_L0X_AGL" value="1"/>
<define name="USE_MATEKSYS_3901_L0X_OPTICAL_FLOW" value="1"/>
<define name="MATEKSYS_3901_L0X_COMPENSATE_ROTATION" value="0"/>
<file name="mateksys_3901_l0x.c"/> <file name="mateksys_3901_l0x.c"/>
</makefile> </makefile>
conf/userconf/tudelft/conf.xml
+11
View File
@@ -309,6 +309,17 @@
gui_color="blue" gui_color="blue"
release="c52a0b7e581c74b42ecc9f9d712324e3ab1fcc5e" release="c52a0b7e581c74b42ecc9f9d712324e3ab1fcc5e"
/> />
<aircraft
name="Nederquad"
ac_id="89"
airframe="airframes/tudelft/nederquad.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/delft_basic.xml"
settings="[settings/rotorcraft_basic.xml] [settings/control/stabilization_rate.xml] [settings/control/stabilization_indi.xml]"
settings_modules="modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/lidar_lite.xml modules/nav_basic_rotorcraft.xml modules/optical_flow_mateksys_3901_l0x.xml modules/stabilization_indi_simple.xml"
gui_color="#406b937dccd0"
/>
<aircraft <aircraft
name="OrigamiMXS_wifi_indi_stereocam" name="OrigamiMXS_wifi_indi_stereocam"
ac_id="15" ac_id="15"
conf/userconf/tudelft/control_panel.xml
+72
View File
@@ -67,6 +67,78 @@
<program name="PayloadForward" command="sw/ground_segment/python/payload_forward/payload.py"/> <program name="PayloadForward" command="sw/ground_segment/python/payload_forward/payload.py"/>
</section> </section>
<section name="sessions"> <section name="sessions">
<session name="EKF Full">
<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"/>
<arg flag="-layout" constant="bottom_settings.xml"/>
</program>
<program name="Messages"/>
<program name="Real-time Plotter">
<arg flag="-g" constant="800x250-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_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="800x250-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="800x250-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"/>
<arg flag="-n"/>
<arg flag="-g" constant="800x250-0+750"/>
<arg flag="-t" constant="INNOVATION_STATUS"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:INS_EKF2:innov_vel"/>
<arg flag="-c" constant="*:telemetry:INS_EKF2:innov_pos"/>
<arg flag="-c" constant="*:telemetry:INS_EKF2:innov_hgt"/>
<arg flag="-c" constant="*:telemetry:INS_EKF2:innov_tas"/>
<arg flag="-c" constant="*:telemetry:INS_EKF2:innov_hagl"/>
<arg flag="-c" constant="*:telemetry:INS_EKF2:innov_flow"/>
<arg flag="-c" constant="*:telemetry:INS_EKF2:innov_beta"/>
<arg flag="-n"/>
<arg flag="-g" constant="800x250-1000+0"/>
<arg flag="-t" constant="POSITION"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:ROTORCRAFT_FP:east:0.0039063"/>
<arg flag="-c" constant="*:telemetry:ROTORCRAFT_FP:north:0.0039063"/>
<arg flag="-c" constant="*:telemetry:ROTORCRAFT_FP:up:0.0039063"/>
<arg flag="-n"/>
<arg flag="-g" constant="800x250-1000+250"/>
<arg flag="-t" constant="VELOCITY"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:ROTORCRAFT_FP:veast:0.0000019"/>
<arg flag="-c" constant="*:telemetry:ROTORCRAFT_FP:vnorth:0.0000019"/>
<arg flag="-n"/>
<arg flag="-g" constant="800x250-1000+500"/>
<arg flag="-t" constant="OPTICAL FLOW"/>
<arg flag="-u" constant="0.05"/>
<arg flag="-c" constant="0.00"/>
<arg flag="-c" constant="*:telemetry:OPTICAL_FLOW:flow_x"/>
<arg flag="-c" constant="*:telemetry:OPTICAL_FLOW:flow_x"/>
<arg flag="-c" constant="*:telemetry:OPTICAL_FLOW:distance_compensated"/>
</program>
</session>
<session name="helidd"> <session name="helidd">
<program name="BluegigaUsbDongle"> <program name="BluegigaUsbDongle">
<arg flag="/dev/ttyACM0" constant="00:07:80:2d:d6:d9"/> <arg flag="/dev/ttyACM0" constant="00:07:80:2d:d6:d9"/>
sw/airborne/modules/optical_flow/mateksys_3901_l0x.c
+89 -48
View File
@@ -25,6 +25,7 @@
* *
*/ */
#include <stdlib.h>
#include "mateksys_3901_l0x.h" #include "mateksys_3901_l0x.h"
#include "mcu_periph/uart.h" #include "mcu_periph/uart.h"
#include "subsystems/abi.h" #include "subsystems/abi.h"
@@ -39,23 +40,39 @@
// Define configuration parameters // Define configuration parameters
#ifndef MATEKSYS_3901_L0X_MOTION_THRES #ifndef MATEKSYS_3901_L0X_MOTION_THRES
#define MATEKSYS_3901_L0X_MOTION_THRES #define MATEKSYS_3901_L0X_MOTION_THRES 100
#endif #endif
#ifndef MATEKSYS_3901_L0X_DISTANCE_THRES #ifndef MATEKSYS_3901_L0X_DISTANCE_THRES
#define MATEKSYS_3901_L0X_DISTANCE_THRES #define MATEKSYS_3901_L0X_DISTANCE_THRES 200
#endif
#ifndef MATEKSYS_3901_L0X_MAX_FLOW
#define MATEKSYS_3901_L0X_MAX_FLOW 300
#endif
#ifndef MATEKSYS_3901_L0X_MAX_DISTANCE
#define MATEKSYS_3901_L0X_MAX_DISTANCE 3000
#endif #endif
#ifndef USE_MATEKSYS_3901_L0X_AGL #ifndef USE_MATEKSYS_3901_L0X_AGL
#define USE_MATEKSYS_3901_L0X_AGL #define USE_MATEKSYS_3901_L0X_AGL 1
#endif #endif
#ifndef USE_MATEKSYS_3901_L0X_OPTICAL_FLOW #ifndef USE_MATEKSYS_3901_L0X_OPTICAL_FLOW
#define USE_MATEKSYS_3901_L0X_OPTICAL_FLOW #define USE_MATEKSYS_3901_L0X_OPTICAL_FLOW 1
#endif #endif
#ifndef MATEKSYS_3901_L0X_COMPENSATE_ROTATION #ifndef MATEKSYS_3901_L0X_COMPENSATE_ROTATION
#define MATEKSYS_3901_L0X_COMPENSATE_ROTATION #define MATEKSYS_3901_L0X_COMPENSATE_ROTATION 1
#endif
#ifndef MATEKSYS_3901_L0X_FLOW_X_SCALER
#define MATEKSYS_3901_L0X_FLOW_X_SCALER 1
#endif
#ifndef MATEKSYS_3901_L0X_FLOW_Y_SCALER
#define MATEKSYS_3901_L0X_FLOW_Y_SCALER 1
#endif #endif
struct Mateksys3901l0X mateksys3901l0x = { struct Mateksys3901l0X mateksys3901l0x = {
@@ -73,15 +90,16 @@ static void mateksys3901l0x_parse(uint8_t byte);
static void mateksys3901l0x_send_optical_flow(struct transport_tx *trans, struct link_device *dev) static void mateksys3901l0x_send_optical_flow(struct transport_tx *trans, struct link_device *dev)
{ {
pprz_msg_send_OPTICAL_FLOW(trans, dev, AC_ID, pprz_msg_send_OPTICAL_FLOW(trans, dev, AC_ID,
&mateksys3901l0x.time_sec, &mateksys3901l0x.time_usec,
&mateksys3901l0x.sensor_id, &mateksys3901l0x.sensor_id,
&mateksys3901l0x.motionX_clean, &mateksys3901l0x.motionX,
&mateksys3901l0x.motionY_clean, &mateksys3901l0x.motionY,
&mateksys3901l0x.velocityX, &mateksys3901l0x.velocityX,
&mateksys3901l0x.velocityY, &mateksys3901l0x.velocityY,
&mateksys3901l0x.motion_quality, &mateksys3901l0x.motion_quality,
&mateksys3901l0x.distance_clean, &mateksys3901l0x.distancemm,
&mateksys3901l0x.distancemm_quality); &mateksys3901l0x.distance_compensated,
&mateksys3901l0x.distancemm_quality);
} }
#endif #endif
@@ -93,18 +111,40 @@ void mateksys3901l0x_init(void)
{ {
mateksys3901l0x.device = &((MATEKSYS_3901_L0X_PORT).device); mateksys3901l0x.device = &((MATEKSYS_3901_L0X_PORT).device);
mateksys3901l0x.parse_crc = 0; mateksys3901l0x.parse_crc = 0;
mateksys3901l0x.motion_quality = 0; mateksys3901l0x.motion_quality = 0;
mateksys3901l0x.motionX = 0; mateksys3901l0x.motionX_temp = 0;
mateksys3901l0x.motionX = 0;
mateksys3901l0x.motionY_temp = 0;
mateksys3901l0x.motionY = 0; mateksys3901l0x.motionY = 0;
mateksys3901l0x.distancemm_quality = 0; mateksys3901l0x.distancemm_quality = 0;
mateksys3901l0x.distancemm = 0; mateksys3901l0x.distancemm_temp = 0;
mateksys3901l0x.distancemm = 0;
mateksys3901l0x.distance_compensated = 0;
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD; mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
mateksys3901l0x.scaler_x = MATEKSYS_3901_L0X_FLOW_X_SCALER;
mateksys3901l0x.scaler_y = MATEKSYS_3901_L0X_FLOW_Y_SCALER;
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTICAL_FLOW, mateksys3901l0x_send_optical_flow); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTICAL_FLOW, mateksys3901l0x_send_optical_flow);
#endif #endif
} }
/**
* Scale the Flow X
*/
void mateksys_3901_l0x_scale_X(float scalex)
{
mateksys3901l0x.scaler_x = scalex;
}
/**
* Scale the Flow Y
*/
void mateksys_3901_l0x_scale_Y(float scaley)
{
mateksys3901l0x.scaler_y = scaley;
}
/** /**
* Receive bytes from the UART port and parse them * Receive bytes from the UART port and parse them
*/ */
@@ -205,26 +245,26 @@ static void mateksys3901l0x_parse(uint8_t byte)
if (mateksys3901l0x.distancemm_quality <= MATEKSYS_3901_L0X_DISTANCE_THRES) { if (mateksys3901l0x.distancemm_quality <= MATEKSYS_3901_L0X_DISTANCE_THRES) {
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD; mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
} else { } else {
mateksys3901l0x.distancemm = byte; mateksys3901l0x.distancemm_temp = byte;
mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++; mateksys3901l0x.parse_status++;
} }
break; break;
case MATEKSYS_3901_L0X_PARSE_DISTANCE_B2: case MATEKSYS_3901_L0X_PARSE_DISTANCE_B2:
mateksys3901l0x.distancemm |= (byte << 8); mateksys3901l0x.distancemm_temp |= (byte << 8);
mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++; mateksys3901l0x.parse_status++;
break; break;
case MATEKSYS_3901_L0X_PARSE_DISTANCE_B3: case MATEKSYS_3901_L0X_PARSE_DISTANCE_B3:
mateksys3901l0x.distancemm |= (byte << 16); mateksys3901l0x.distancemm_temp |= (byte << 16);
mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++; mateksys3901l0x.parse_status++;
break; break;
case MATEKSYS_3901_L0X_PARSE_DISTANCE_B4: case MATEKSYS_3901_L0X_PARSE_DISTANCE_B4:
mateksys3901l0x.distancemm |= (byte << 24); mateksys3901l0x.distancemm_temp |= (byte << 24);
mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_CHECKSUM; mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_CHECKSUM;
break; break;
@@ -240,50 +280,50 @@ static void mateksys3901l0x_parse(uint8_t byte)
if (mateksys3901l0x.motion_quality <= MATEKSYS_3901_L0X_MOTION_THRES) { if (mateksys3901l0x.motion_quality <= MATEKSYS_3901_L0X_MOTION_THRES) {
mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD; mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
} else { } else {
mateksys3901l0x.motionY = byte; mateksys3901l0x.motionY_temp = byte;
mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++; mateksys3901l0x.parse_status++;
} }
break; break;
case MATEKSYS_3901_L0X_PARSE_MOTIONY_B2: case MATEKSYS_3901_L0X_PARSE_MOTIONY_B2:
mateksys3901l0x.motionY |= (byte << 8); mateksys3901l0x.motionY_temp |= (byte << 8);
mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++; mateksys3901l0x.parse_status++;
break; break;
case MATEKSYS_3901_L0X_PARSE_MOTIONY_B3: case MATEKSYS_3901_L0X_PARSE_MOTIONY_B3:
mateksys3901l0x.motionY |= (byte << 16); mateksys3901l0x.motionY_temp |= (byte << 16);
mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++; mateksys3901l0x.parse_status++;
break; break;
case MATEKSYS_3901_L0X_PARSE_MOTIONY_B4: case MATEKSYS_3901_L0X_PARSE_MOTIONY_B4:
mateksys3901l0x.motionY |= (byte << 24); mateksys3901l0x.motionY_temp |= (byte << 24);
mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++; mateksys3901l0x.parse_status++;
break; break;
case MATEKSYS_3901_L0X_PARSE_MOTIONX_B1: case MATEKSYS_3901_L0X_PARSE_MOTIONX_B1:
mateksys3901l0x.motionX = byte; mateksys3901l0x.motionX_temp = byte;
mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++; mateksys3901l0x.parse_status++;
break; break;
case MATEKSYS_3901_L0X_PARSE_MOTIONX_B2: case MATEKSYS_3901_L0X_PARSE_MOTIONX_B2:
mateksys3901l0x.motionX |= (byte << 8); mateksys3901l0x.motionX_temp |= (byte << 8);
mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++; mateksys3901l0x.parse_status++;
break; break;
case MATEKSYS_3901_L0X_PARSE_MOTIONX_B3: case MATEKSYS_3901_L0X_PARSE_MOTIONX_B3:
mateksys3901l0x.motionX |= (byte << 16); mateksys3901l0x.motionX_temp |= (byte << 16);
mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++; mateksys3901l0x.parse_status++;
break; break;
case MATEKSYS_3901_L0X_PARSE_MOTIONX_B4: case MATEKSYS_3901_L0X_PARSE_MOTIONX_B4:
mateksys3901l0x.motionX |= (byte << 24); mateksys3901l0x.motionX_temp |= (byte << 24);
mateksys3901l0x.parse_crc += byte; mateksys3901l0x.parse_crc += byte;
mateksys3901l0x.parse_status++; mateksys3901l0x.parse_status++;
break; break;
@@ -291,41 +331,42 @@ static void mateksys3901l0x_parse(uint8_t byte)
case MATEKSYS_3901_L0X_PARSE_CHECKSUM: case MATEKSYS_3901_L0X_PARSE_CHECKSUM:
// When the distance and motion info are valid (max values based on sensor specifications)... // When the distance and motion info are valid (max values based on sensor specifications)...
if (mateksys3901l0x.distancemm > 0 && mateksys3901l0x.distancemm <= 3000 && abs(mateksys3901l0x.motionX) <= 300 && abs(mateksys3901l0x.motionY) <= 300) { if (mateksys3901l0x.distancemm_temp > 0 && mateksys3901l0x.distancemm_temp <= MATEKSYS_3901_L0X_MAX_DISTANCE && abs(mateksys3901l0x.motionX_temp) <= MATEKSYS_3901_L0X_MAX_FLOW && abs(mateksys3901l0x.motionY_temp) <= MATEKSYS_3901_L0X_MAX_FLOW) {
// ... compensate AGL measurement for body rotation
// pass temporary message and apply calibration parameters
mateksys3901l0x.motionX = mateksys3901l0x.motionX_temp * mateksys3901l0x.scaler_x;
mateksys3901l0x.motionY = mateksys3901l0x.motionY_temp * mateksys3901l0x.scaler_y;
mateksys3901l0x.distancemm = mateksys3901l0x.distancemm_temp;
// get from ground distance to altitude by compensating for body rotation
if (MATEKSYS_3901_L0X_COMPENSATE_ROTATION) { if (MATEKSYS_3901_L0X_COMPENSATE_ROTATION) {
float phi = stateGetNedToBodyEulers_f()->phi; float phi = stateGetNedToBodyEulers_f()->phi;
float theta = stateGetNedToBodyEulers_f()->theta; float theta = stateGetNedToBodyEulers_f()->theta;
float gain = (float)fabs((double)(cosf(phi) * cosf(theta))); mateksys3901l0x.distance_compensated = ((mateksys3901l0x.distancemm * cos(phi)) * cos(theta)) * 0.001;
mateksys3901l0x.distancemm = mateksys3901l0x.distancemm * gain;
} }
// send messages with no error measurements // estimate velocity and send it to telemetry (flow not compensated for gyro measurements)
mateksys3901l0x.distance_clean = mateksys3901l0x.distancemm/1000.f; mateksys3901l0x.velocityX = mateksys3901l0x.distance_compensated * sin(RadOfDeg(mateksys3901l0x.motionY)); // velocity in m/sec
mateksys3901l0x.motionX_clean = mateksys3901l0x.motionX; mateksys3901l0x.velocityY = mateksys3901l0x.distance_compensated * sin(RadOfDeg(mateksys3901l0x.motionX)); // velocity in m/sec
mateksys3901l0x.motionY_clean = mateksys3901l0x.motionY;
// estimate velocity and send it to telemetry
mateksys3901l0x.velocityX = mateksys3901l0x.distance_clean * sin(RadOfDeg(mateksys3901l0x.motionX_clean));
mateksys3901l0x.velocityY = mateksys3901l0x.distance_clean * sin(RadOfDeg(mateksys3901l0x.motionY_clean));
// get ticks // get ticks
uint32_t now_ts = get_sys_time_usec(); mateksys3901l0x.time_usec = get_sys_time_usec();
mateksys3901l0x.time_sec = now_ts*1e-6;
// send AGL (if requested) // send AGL (if requested)
if (USE_MATEKSYS_3901_L0X_AGL) { if (USE_MATEKSYS_3901_L0X_AGL) {
AbiSendMsgAGL(AGL_LIDAR_MATEKSYS_3901_L0X_ID, AbiSendMsgAGL(AGL_LIDAR_MATEKSYS_3901_L0X_ID,
now_ts, mateksys3901l0x.time_usec,
mateksys3901l0x.distance_clean); mateksys3901l0x.distance_compensated);
} }
// send optical flow (if requested) // send optical flow (if requested)
if (USE_MATEKSYS_3901_L0X_OPTICAL_FLOW) { if (USE_MATEKSYS_3901_L0X_OPTICAL_FLOW) {
AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_MATEKSYS_3901_L0X_ID, AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_MATEKSYS_3901_L0X_ID,
now_ts, mateksys3901l0x.time_usec,
mateksys3901l0x.motionX_clean, mateksys3901l0x.motionX, // motion in deg/sec
mateksys3901l0x.motionY_clean, mateksys3901l0x.motionY, // motion in deg/sec
0, 0,
0, 0,
mateksys3901l0x.motion_quality, mateksys3901l0x.motion_quality,
sw/airborne/modules/optical_flow/mateksys_3901_l0x.h
+19 -16
View File
@@ -22,7 +22,6 @@
/** @file modules/optical_flow/mateksys_3901_l0x.h /** @file modules/optical_flow/mateksys_3901_l0x.h
* @brief Driver for the mateksys_3901_l0x sensor via MSPx protocol output * @brief Driver for the mateksys_3901_l0x sensor via MSPx protocol output
*
*/ */
/* /*
@@ -37,7 +36,6 @@ https://github.com/iNavFlight/inav/wiki/MSP-V2
#include "std.h" #include "std.h"
#include "stdbool.h" #include "stdbool.h"
//#include "filters/median_filter.h" //Who knows we need it ;)
#define MSP2_IS_SENSOR_MESSAGE(x) ((x) >= 0x1F00 && (x) <= 0x1FFF) #define MSP2_IS_SENSOR_MESSAGE(x) ((x) >= 0x1F00 && (x) <= 0x1FFF)
@@ -45,7 +43,7 @@ https://github.com/iNavFlight/inav/wiki/MSP-V2
#define MSP2_SENSOR_OPTIC_FLOW 0x1F02 #define MSP2_SENSOR_OPTIC_FLOW 0x1F02
enum Mateksys3901l0XParseStatus { enum Mateksys3901l0XParseStatus {
MATEKSYS_3901_L0X_INITIALIZE, // initialization MATEKSYS_3901_L0X_INITIALIZE,
MATEKSYS_3901_L0X_PARSE_HEAD, MATEKSYS_3901_L0X_PARSE_HEAD,
MATEKSYS_3901_L0X_PARSE_HEAD2, MATEKSYS_3901_L0X_PARSE_HEAD2,
MATEKSYS_3901_L0X_PARSE_DIRECTION, MATEKSYS_3901_L0X_PARSE_DIRECTION,
@@ -53,13 +51,13 @@ enum Mateksys3901l0XParseStatus {
MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B1, MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B1,
MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B2, MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B2,
MATEKSYS_3901_L0X_PARSE_SIZE, MATEKSYS_3901_L0X_PARSE_SIZE,
MATEKSYS_3901_L0X_PARSE_POINTER, // ?? MATEKSYS_3901_L0X_PARSE_POINTER,
MATEKSYS_3901_L0X_PARSE_DISTANCEQUALITY, // used if lidar message MATEKSYS_3901_L0X_PARSE_DISTANCEQUALITY,
MATEKSYS_3901_L0X_PARSE_DISTANCE_B1, MATEKSYS_3901_L0X_PARSE_DISTANCE_B1,
MATEKSYS_3901_L0X_PARSE_DISTANCE_B2, MATEKSYS_3901_L0X_PARSE_DISTANCE_B2,
MATEKSYS_3901_L0X_PARSE_DISTANCE_B3, MATEKSYS_3901_L0X_PARSE_DISTANCE_B3,
MATEKSYS_3901_L0X_PARSE_DISTANCE_B4, MATEKSYS_3901_L0X_PARSE_DISTANCE_B4,
MATEKSYS_3901_L0X_PARSE_MOTIONQUALITY, // used if flow message MATEKSYS_3901_L0X_PARSE_MOTIONQUALITY,
MATEKSYS_3901_L0X_PARSE_MOTIONY_B1, MATEKSYS_3901_L0X_PARSE_MOTIONY_B1,
MATEKSYS_3901_L0X_PARSE_MOTIONY_B2, MATEKSYS_3901_L0X_PARSE_MOTIONY_B2,
MATEKSYS_3901_L0X_PARSE_MOTIONY_B3, MATEKSYS_3901_L0X_PARSE_MOTIONY_B3,
@@ -74,19 +72,22 @@ enum Mateksys3901l0XParseStatus {
struct Mateksys3901l0X { struct Mateksys3901l0X {
struct link_device *device; struct link_device *device;
enum Mateksys3901l0XParseStatus parse_status; enum Mateksys3901l0XParseStatus parse_status;
float time_sec; float time_usec;
uint8_t sensor_id; uint8_t sensor_id;
uint8_t motion_quality; uint8_t motion_quality;
int32_t motionX_temp;
int32_t motionX; int32_t motionX;
int32_t motionY_temp;
int32_t motionY; int32_t motionY;
int32_t motionX_clean; uint8_t distancemm_quality;
int32_t motionY_clean; int32_t distancemm_temp;
uint8_t distancemm_quality; float distancemm;
int32_t distancemm; float distance_compensated;
float distance_clean; float velocityX;
float velocityX; float velocityY;
float velocityY; uint8_t parse_crc;
uint8_t parse_crc; float scaler_x;
float scaler_y;
}; };
extern struct Mateksys3901l0X mateksys3901l0x; extern struct Mateksys3901l0X mateksys3901l0x;
@@ -94,6 +95,8 @@ extern struct Mateksys3901l0X mateksys3901l0x;
extern void mateksys3901l0x_init(void); extern void mateksys3901l0x_init(void);
extern void mateksys3901l0x_event(void); extern void mateksys3901l0x_event(void);
extern void mateksys3901l0x_downlink(void); extern void mateksys3901l0x_downlink(void);
extern void mateksys_3901_l0x_scale_X(float scalex);
extern void mateksys_3901_l0x_scale_Y(float scaley);
#endif /* MATEKSYS_3901_L0X_H */ #endif /* MATEKSYS_3901_L0X_H */
sw/airborne/modules/optical_flow/px4flow.c
+2
View File
@@ -102,6 +102,7 @@ static void decode_optical_flow_msg(struct mavlink_message *msg __attribute__((u
float phi = stateGetNedToBodyEulers_f()->phi; float phi = stateGetNedToBodyEulers_f()->phi;
float theta = stateGetNedToBodyEulers_f()->theta; float theta = stateGetNedToBodyEulers_f()->theta;
float gain = (float)fabs( (double) (cosf(phi) * cosf(theta))); float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
optical_flow.distance = optical_flow.ground_distance;
optical_flow.ground_distance = optical_flow.ground_distance / gain; optical_flow.ground_distance = optical_flow.ground_distance / gain;
} }
@@ -172,6 +173,7 @@ void px4flow_downlink(void)
&optical_flow.flow_comp_m_x, &optical_flow.flow_comp_m_x,
&optical_flow.flow_comp_m_y, &optical_flow.flow_comp_m_y,
&optical_flow.quality, &optical_flow.quality,
&optical_flow.distance,
&optical_flow.ground_distance, &optical_flow.ground_distance,
&distance_quality); &distance_quality);
} }
sw/airborne/modules/optical_flow/px4flow.h
+1
View File
@@ -47,6 +47,7 @@ struct mavlink_optical_flow {
float flow_comp_m_x; ///< Flow in meters in x-sensor direction, angular-speed compensated [meters/sec] float flow_comp_m_x; ///< Flow in meters in x-sensor direction, angular-speed compensated [meters/sec]
float flow_comp_m_y; ///< Flow in meters in y-sensor direction, angular-speed compensated [meters/sec] float flow_comp_m_y; ///< Flow in meters in y-sensor direction, angular-speed compensated [meters/sec]
float ground_distance; ///< Ground distance in meters. Positive value: distance known. Negative value: Unknown distance float ground_distance; ///< Ground distance in meters. Positive value: distance known. Negative value: Unknown distance
float distance; ///< Distance measured without compensation in meters
int32_t flow_x; ///< Flow in pixels in x-sensor direction int32_t flow_x; ///< Flow in pixels in x-sensor direction
int32_t flow_y; ///< Flow in pixels in y-sensor direction int32_t flow_y; ///< Flow in pixels in y-sensor direction
uint8_t sensor_id; ///< Sensor ID uint8_t sensor_id; ///< Sensor ID
sw/airborne/modules/optical_flow/px4flow_i2c.c
+3
View File
@@ -278,6 +278,7 @@ void px4flow_i2c_downlink(void)
uint8_t quality = px4flow.i2c_int_frame.qual; uint8_t quality = px4flow.i2c_int_frame.qual;
float ground_distance = ((float)px4flow.i2c_int_frame.ground_distance) / 1000.0; float ground_distance = ((float)px4flow.i2c_int_frame.ground_distance) / 1000.0;
float distance = 0;
#else #else
int32_t flow_x = px4flow.i2c_frame.pixel_flow_x_sum; int32_t flow_x = px4flow.i2c_frame.pixel_flow_x_sum;
int32_t flow_y = px4flow.i2c_frame.pixel_flow_y_sum; int32_t flow_y = px4flow.i2c_frame.pixel_flow_y_sum;
@@ -287,6 +288,7 @@ void px4flow_i2c_downlink(void)
uint8_t quality = px4flow.i2c_frame.qual; uint8_t quality = px4flow.i2c_frame.qual;
float ground_distance = ((float)px4flow.i2c_frame.ground_distance) / 1000.0; float ground_distance = ((float)px4flow.i2c_frame.ground_distance) / 1000.0;
float distance;
#endif #endif
DOWNLINK_SEND_OPTICAL_FLOW(DefaultChannel, DefaultDevice, DOWNLINK_SEND_OPTICAL_FLOW(DefaultChannel, DefaultDevice,
@@ -298,5 +300,6 @@ void px4flow_i2c_downlink(void)
&flow_comp_m_y, &flow_comp_m_y,
&quality, &quality,
&ground_distance, &ground_distance,
&distance,
&distance_quality); &distance_quality);
} }
sw/airborne/subsystems/abi_sender_ids.h
+4
View File
@@ -209,6 +209,10 @@
#define MAG_RM3100_SENDER_ID 5 #define MAG_RM3100_SENDER_ID 5
#endif #endif
#ifndef MAG_DATALINK_SENDER_ID
#define MAG_DATALINK_SENDER_ID 6
#endif
#ifndef IMU_MAG_PITOT_ID #ifndef IMU_MAG_PITOT_ID
#define IMU_MAG_PITOT_ID 50 #define IMU_MAG_PITOT_ID 50
#endif #endif
sw/airborne/subsystems/gps/gps_datalink.c
+43 -2
View File
@@ -1,3 +1,4 @@
/* /*
* Copyright (C) 2014 Freek van Tienen * Copyright (C) 2014 Freek van Tienen
* *
@@ -31,7 +32,15 @@
#include "subsystems/gps.h" #include "subsystems/gps.h"
#include "subsystems/abi.h" #include "subsystems/abi.h"
#include "subsystems/imu.h"
#include "subsystems/datalink/datalink.h" #include "subsystems/datalink/datalink.h"
#include "subsystems/datalink/downlink.h"
/** Set to 1 to receive also magnetometer ABI messages */
#ifndef GPS_DATALINK_USE_MAG
#define GPS_DATALINK_USE_MAG 1
#endif
PRINT_CONFIG_VAR(GPS_DATALINK_USE_MAG)
struct LtpDef_i ltp_def; struct LtpDef_i ltp_def;
@@ -57,6 +66,26 @@ void gps_datalink_init(void)
ltp_def_from_lla_i(&ltp_def, &llh_nav0); ltp_def_from_lla_i(&ltp_def, &llh_nav0);
} }
// Send GPS heading info as magnetometer messages
static void send_magnetometer(int32_t course, uint32_t now_ts)
{
struct Int32Vect3 mag;
struct FloatVect3 mag_real;
// course from gps in [0, 2*Pi]*1e7 (CW/north)
float heading = course/1e7;
mag_real.x = cos(heading);
mag_real.y = -sin(heading);
mag_real.z = 0;
MAGS_BFP_OF_REAL(mag, mag_real);
// update IMU information
VECT3_COPY(imu.mag_unscaled, mag);
imu_scale_mag(&imu);
// Send fake ABI for GPS, Magnetometer and Optical Flow for GPS fusion
AbiSendMsgIMU_MAG_INT32(MAG_DATALINK_SENDER_ID, now_ts, &mag);
}
// Parse the REMOTE_GPS_SMALL datalink packet // Parse the REMOTE_GPS_SMALL datalink packet
static void parse_gps_datalink_small(int16_t heading, uint32_t pos_xyz, uint32_t speed_xyz, uint32_t tow) static void parse_gps_datalink_small(int16_t heading, uint32_t pos_xyz, uint32_t speed_xyz, uint32_t tow)
{ {
@@ -168,8 +197,14 @@ static void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, in
gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem; gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem;
gps_datalink.last_3dfix_time = sys_time.nb_sec; gps_datalink.last_3dfix_time = sys_time.nb_sec;
// publish new GPS data
uint32_t now_ts = get_sys_time_usec(); uint32_t now_ts = get_sys_time_usec();
// if selected, publish magnetometer data
#if GPS_DATALINK_USE_MAG
send_magnetometer(course, now_ts);
#endif
// publish new GPS data
AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink); AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink);
} }
@@ -223,8 +258,14 @@ static void parse_gps_datalink_local(float enu_x, float enu_y, float enu_z,
gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem; gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem;
gps_datalink.last_3dfix_time = sys_time.nb_sec; gps_datalink.last_3dfix_time = sys_time.nb_sec;
// publish new GPS data
uint32_t now_ts = get_sys_time_usec(); uint32_t now_ts = get_sys_time_usec();
// if selected, publish magnetometer data
#if GPS_DATALINK_USE_MAG
send_magnetometer(course, now_ts);
#endif
// Publish GPS data
AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink); AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink);
} }
sw/airborne/subsystems/ins/ins_ekf2.cpp
+196 -22
View File
@@ -72,16 +72,22 @@ PRINT_CONFIG_VAR(INS_EKF2_GPS_CHECK_MASK)
PRINT_CONFIG_VAR(INS_EKF2_AGL_ID) PRINT_CONFIG_VAR(INS_EKF2_AGL_ID)
/** Default AGL sensor minimum range */ /** Default AGL sensor minimum range */
#ifndef INS_SONAR_MIN_RANGE #ifndef INS_EKF2_SONAR_MIN_RANGE
#define INS_SONAR_MIN_RANGE 0.001 #define INS_EKF2_SONAR_MIN_RANGE 0.001
#endif #endif
PRINT_CONFIG_VAR(INS_SONAR_MIN_RANGE) PRINT_CONFIG_VAR(INS_EKF2_SONAR_MIN_RANGE)
/** Default AGL sensor maximum range */ /** Default AGL sensor maximum range */
#ifndef INS_SONAR_MAX_RANGE #ifndef INS_EKF2_SONAR_MAX_RANGE
#define INS_SONAR_MAX_RANGE 4.0 #define INS_EKF2_SONAR_MAX_RANGE 4
#endif #endif
PRINT_CONFIG_VAR(INS_SONAR_MAX_RANGE) PRINT_CONFIG_VAR(INS_EKF2_SONAR_MAX_RANGE)
/** If enabled uses radar sensor as primary AGL source, if possible */
#ifndef INS_EKF2_RANGE_MAIN_AGL
#define INS_EKF2_RANGE_MAIN_AGL 1
#endif
PRINT_CONFIG_VAR(INS_EKF2_RANGE_MAIN_AGL)
/** default barometer to use in INS */ /** default barometer to use in INS */
#ifndef INS_EKF2_BARO_ID #ifndef INS_EKF2_BARO_ID
@@ -117,6 +123,66 @@ PRINT_CONFIG_VAR(INS_EKF2_MAG_ID)
#endif #endif
PRINT_CONFIG_VAR(INS_EKF2_GPS_ID) PRINT_CONFIG_VAR(INS_EKF2_GPS_ID)
/* default Optical Flow to use in INS */
#ifndef INS_EKF2_OF_ID
#define INS_EKF2_OF_ID ABI_BROADCAST
#endif
PRINT_CONFIG_VAR(INS_EKF2_OF_ID)
/* Default flow/radar message delay (in ms) */
#ifndef INS_EKF2_FLOW_SENSOR_DELAY
#define INS_EKF2_FLOW_SENSOR_DELAY 15
#endif
PRINT_CONFIG_VAR(INS_FLOW_SENSOR_DELAY)
/* Default minimum accepted quality (1 to 255) */
#ifndef INS_EKF2_MIN_FLOW_QUALITY
#define INS_EKF2_MIN_FLOW_QUALITY 100
#endif
PRINT_CONFIG_VAR(INS_EKF2_MIN_FLOW_QUALITY)
/* Max flow rate that the sensor can measure (rad/sec) */
#ifndef INS_EKF2_MAX_FLOW_RATE
#define INS_EKF2_MAX_FLOW_RATE 200
#endif
PRINT_CONFIG_VAR(INS_EKF2_MAX_FLOW_RATE)
/* Flow sensor X offset from IMU position in meters */
#ifndef INS_EKF2_FLOW_OFFSET_X
#define INS_EKF2_FLOW_OFFSET_X 0
#endif
PRINT_CONFIG_VAR(INS_EKF2_FLOW_OFFSET_X)
/* Flow sensor Y offset from IMU position in meters */
#ifndef INS_EKF2_FLOW_OFFSET_Y
#define INS_EKF2_FLOW_OFFSET_Y 0
#endif
PRINT_CONFIG_VAR(INS_EKF2_FLOW_OFFSET_Y)
/* Flow sensor Z offset from IMU position in meters */
#ifndef INS_EKF2_FLOW_OFFSET_Z
#define INS_EKF2_FLOW_OFFSET_Z 0
#endif
PRINT_CONFIG_VAR(INS_EKF2_FLOW_OFFSET_Z)
/* Flow sensor noise in rad/sec */
#ifndef INS_EKF2_FLOW_NOISE
#define INS_EKF2_FLOW_NOISE 0.03
#endif
PRINT_CONFIG_VAR(INS_EKF2_FLOW_NOISE)
/* Flow sensor noise at qmin in rad/sec */
#ifndef INS_EKF2_FLOW_NOISE_QMIN
#define INS_EKF2_FLOW_NOISE_QMIN 0.05
#endif
PRINT_CONFIG_VAR(INS_EKF2_FLOW_NOISE_QMIN)
/* Flow sensor innovation gate */
#ifndef INS_EKF2_FLOW_INNOV_GATE
#define INS_EKF2_FLOW_INNOV_GATE 4
#endif
PRINT_CONFIG_VAR(INS_EKF2_FLOW_INNOV_GATE)
/* All registered ABI events */ /* All registered ABI events */
static abi_event agl_ev; static abi_event agl_ev;
static abi_event baro_ev; static abi_event baro_ev;
@@ -125,7 +191,11 @@ static abi_event accel_ev;
static abi_event mag_ev; static abi_event mag_ev;
static abi_event gps_ev; static abi_event gps_ev;
static abi_event body_to_imu_ev; static abi_event body_to_imu_ev;
static abi_event optical_flow_ev;
/* Build optical flow and gps message struct based on flow message defined in common.h */
struct gps_message gps_msg = {}; struct gps_message gps_msg = {};
struct flow_message flow_msg = {};
/* All ABI callbacks */ /* All ABI callbacks */
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance); static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
@@ -135,23 +205,43 @@ static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag); static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag);
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s); static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f); static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f);
static void optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence);
/* Main EKF2 structure for keeping track of the status */ /* Main EKF2 structure for keeping track of the status and use cross messaging */
struct ekf2_t { struct ekf2_t
{
// stamp and dt for sensors
uint32_t gyro_stamp; uint32_t gyro_stamp;
uint32_t gyro_dt; uint32_t gyro_dt;
uint32_t accel_stamp; uint32_t accel_stamp;
uint32_t accel_dt; uint32_t accel_dt;
uint32_t flow_stamp;
uint32_t flow_dt;
// gyro and accellerometer values
FloatRates gyro; FloatRates gyro;
FloatVect3 accel; FloatVect3 accel;
bool gyro_valid; bool gyro_valid;
bool accel_valid; bool accel_valid;
uint8_t quat_reset_counter; // optical flow and gyro values
float flow_quality;
float flow_x;
float flow_y;
float gyro_roll;
float gyro_pitch;
float gyro_yaw;
float offset_x;
float offset_y;
float offset_z;
// optical flow takeover
float flow_innov;
uint8_t quat_reset_counter;
uint64_t ltp_stamp; uint64_t ltp_stamp;
struct LtpDef_i ltp_def; struct LtpDef_i ltp_def;
struct OrientationReps body_to_imu; struct OrientationReps body_to_imu;
bool got_imu_data; bool got_imu_data;
}; };
@@ -191,13 +281,30 @@ static void send_ins_ekf2(struct transport_tx *trans, struct link_device *dev)
ekf.get_ekf_soln_status(&soln_status); ekf.get_ekf_soln_status(&soln_status);
uint16_t innov_test_status; uint16_t innov_test_status;
float mag, vel, pos, hgt, tas, hagl, beta, mag_decl; float mag, vel, pos, hgt, tas, hagl, flow, beta, mag_decl;
uint8_t terrain_valid, dead_reckoning;
ekf.get_innovation_test_status(&innov_test_status, &mag, &vel, &pos, &hgt, &tas, &hagl, &beta); ekf.get_innovation_test_status(&innov_test_status, &mag, &vel, &pos, &hgt, &tas, &hagl, &beta);
ekf.get_flow_innov(&flow);
ekf.get_mag_decl_deg(&mag_decl); ekf.get_mag_decl_deg(&mag_decl);
uint32_t fix_status = (control_mode >> 2) & 1;
if (ekf.get_terrain_valid()) {
terrain_valid = 1;
} else {
terrain_valid = 0;
}
if (ekf.inertial_dead_reckoning()) {
dead_reckoning = 1;
} else {
dead_reckoning = 0;
}
pprz_msg_send_INS_EKF2(trans, dev, AC_ID, pprz_msg_send_INS_EKF2(trans, dev, AC_ID,
&control_mode, &filter_fault_status, &gps_check_status, &soln_status, &fix_status, &filter_fault_status, &gps_check_status, &soln_status,
&innov_test_status, &mag, &vel, &pos, &hgt, &tas, &hagl, &beta, &innov_test_status, &mag, &vel, &pos, &hgt, &tas, &hagl, &flow, &beta,
&mag_decl); &mag_decl, &terrain_valid, &dead_reckoning);
} }
static void send_ins_ekf2_ext(struct transport_tx *trans, struct link_device *dev) static void send_ins_ekf2_ext(struct transport_tx *trans, struct link_device *dev)
@@ -210,8 +317,8 @@ static void send_ins_ekf2_ext(struct transport_tx *trans, struct link_device *de
gps_blocked_b = gps_blocked; gps_blocked_b = gps_blocked;
pprz_msg_send_INS_EKF2_EXT(trans, dev, AC_ID, pprz_msg_send_INS_EKF2_EXT(trans, dev, AC_ID,
&gps_drift[0], &gps_drift[1], &gps_drift[2], &gps_blocked_b, &gps_drift[0], &gps_drift[1], &gps_drift[2], &gps_blocked_b,
&vibe[0], &vibe[1], &vibe[2]); &vibe[0], &vibe[1], &vibe[2]);
} }
static void send_filter_status(struct transport_tx *trans, struct link_device *dev) static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
@@ -258,7 +365,7 @@ static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
ekf.get_gyro_bias(gyro_bias); ekf.get_gyro_bias(gyro_bias);
ekf.get_state_delayed(states); ekf.get_state_delayed(states);
pprz_msg_send_AHRS_BIAS(trans, dev, AC_ID, &accel_bias[0], &accel_bias[1], &accel_bias[2], pprz_msg_send_AHRS_BIAS(trans, dev, AC_ID, &accel_bias[0], &accel_bias[1], &accel_bias[2],
&gyro_bias[0], &gyro_bias[1], &gyro_bias[2], &states[19], &states[20], &states[21]); &gyro_bias[0], &gyro_bias[1], &gyro_bias[2], &states[19], &states[20], &states[21]);
} }
#endif #endif
@@ -274,17 +381,38 @@ void ins_ekf2_init(void)
ekf_params->vdist_sensor_type = INS_EKF2_VDIST_SENSOR_TYPE; ekf_params->vdist_sensor_type = INS_EKF2_VDIST_SENSOR_TYPE;
ekf_params->gps_check_mask = INS_EKF2_GPS_CHECK_MASK; ekf_params->gps_check_mask = INS_EKF2_GPS_CHECK_MASK;
/* Set optical flow parameters */
ekf_params->flow_qual_min = INS_EKF2_MIN_FLOW_QUALITY;
ekf_params->flow_delay_ms = INS_EKF2_FLOW_SENSOR_DELAY;
ekf_params->range_delay_ms = INS_EKF2_FLOW_SENSOR_DELAY;
ekf_params->flow_noise = INS_EKF2_FLOW_NOISE;
ekf_params->flow_noise_qual_min = INS_EKF2_FLOW_NOISE_QMIN;
ekf_params->flow_innov_gate = INS_EKF2_FLOW_INNOV_GATE;
/* Set flow sensor offset from IMU position in xyz (m) */
ekf2.offset_x = INS_EKF2_FLOW_OFFSET_X;
ekf2.offset_y = INS_EKF2_FLOW_OFFSET_Y;
ekf2.offset_z = INS_EKF2_FLOW_OFFSET_Z;
ekf_params->flow_pos_body = {0.001f*ekf2.offset_x, 0.001f*ekf2.offset_y, 0.001f*ekf2.offset_z};
/* Set range as default AGL measurement if possible */
ekf_params->range_aid = INS_EKF2_RANGE_MAIN_AGL;
/* Initialize struct */ /* Initialize struct */
ekf2.ltp_stamp = 0; ekf2.ltp_stamp = 0;
ekf2.accel_stamp = 0; ekf2.accel_stamp = 0;
ekf2.gyro_stamp = 0; ekf2.gyro_stamp = 0;
ekf2.flow_stamp = 0;
ekf2.gyro_valid = false; ekf2.gyro_valid = false;
ekf2.accel_valid = false; ekf2.accel_valid = false;
ekf2.got_imu_data = false; ekf2.got_imu_data = false;
ekf2.quat_reset_counter = 0; ekf2.quat_reset_counter = 0;
/* Initialize the range sensor limits */ /* Initialize the range sensor limits */
ekf.set_rangefinder_limits(INS_SONAR_MIN_RANGE, INS_SONAR_MAX_RANGE); ekf.set_rangefinder_limits(INS_EKF2_SONAR_MIN_RANGE, INS_EKF2_SONAR_MAX_RANGE);
/* Initialize the flow sensor limits */
ekf.set_optical_flow_limits(INS_EKF2_MAX_FLOW_RATE, INS_EKF2_SONAR_MIN_RANGE, INS_EKF2_SONAR_MAX_RANGE);
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_REF, send_ins_ref); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_REF, send_ins_ref);
@@ -305,6 +433,7 @@ void ins_ekf2_init(void)
AbiBindMsgIMU_MAG_INT32(INS_EKF2_MAG_ID, &mag_ev, mag_cb); AbiBindMsgIMU_MAG_INT32(INS_EKF2_MAG_ID, &mag_ev, mag_cb);
AbiBindMsgGPS(INS_EKF2_GPS_ID, &gps_ev, gps_cb); AbiBindMsgGPS(INS_EKF2_GPS_ID, &gps_ev, gps_cb);
AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
AbiBindMsgOPTICAL_FLOW(INS_EKF2_OF_ID, &optical_flow_ev, optical_flow_cb);
} }
/* Update the INS state */ /* Update the INS state */
@@ -386,12 +515,22 @@ void ins_ekf2_update(void)
ekf2.got_imu_data = false; ekf2.got_imu_data = false;
} }
void ins_ekf2_change_param(int32_t unk) { void ins_ekf2_change_param(int32_t unk)
{
ekf_params->mag_fusion_type = ekf2_params.mag_fusion_type = unk; ekf_params->mag_fusion_type = ekf2_params.mag_fusion_type = unk;
} }
void ins_ekf2_remove_gps(int32_t mode)
{
if (mode) {
ekf_params->fusion_mode = ekf2_params.fusion_mode = (MASK_USE_OF | MASK_USE_GPSYAW);
} else {
ekf_params->fusion_mode = ekf2_params.fusion_mode = INS_EKF2_FUSION_MODE;
}
}
/** Publish the attitude and get the new state /** Publish the attitude and get the new state
* Directly called after a succeslfull gyro+accel reading * Directly called after a succeslfull gyro+accel reading
*/ */
static void ins_ekf2_publish_attitude(uint32_t stamp) static void ins_ekf2_publish_attitude(uint32_t stamp)
{ {
@@ -471,12 +610,12 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, f
{ {
// Calculate the air density // Calculate the air density
float rho = pprz_isa_density_of_pressure(pressure, float rho = pprz_isa_density_of_pressure(pressure,
20.0f); // TODO: add temperature compensation now set to 20 degree celcius 20.0f); // TODO: add temperature compensation now set to 20 degree celcius
ekf.set_air_density(rho); ekf.set_air_density(rho);
// Calculate the height above mean sea level based on pressure // Calculate the height above mean sea level based on pressure
float height_amsl_m = pprz_isa_height_of_pressure_full(pressure, float height_amsl_m = pprz_isa_height_of_pressure_full(pressure,
101325.0); //101325.0 defined as PPRZ_ISA_SEA_LEVEL_PRESSURE in pprz_isa.h 101325.0); //101325.0 defined as PPRZ_ISA_SEA_LEVEL_PRESSURE in pprz_isa.h
ekf.setBaroData(stamp, height_amsl_m); ekf.setBaroData(stamp, height_amsl_m);
} }
@@ -602,3 +741,38 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
{ {
orientationSetQuat_f(&ekf2.body_to_imu, q_b2i_f); orientationSetQuat_f(&ekf2.body_to_imu, q_b2i_f);
} }
/* Update INS based on Optical Flow information */
static void optical_flow_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp,
int32_t flow_x,
int32_t flow_y,
int32_t flow_der_x __attribute__((unused)),
int32_t flow_der_y __attribute__((unused)),
float quality,
float size_divergence __attribute__((unused)))
{
// update time
ekf2.flow_dt = stamp - ekf2.flow_stamp;
ekf2.flow_stamp = stamp;
/* Build integrated flow and gyro messages for filter
NOTE: pure rotations should result in same flow_x and
gyro_roll and same flow_y and gyro_pitch */
ekf2.flow_quality = quality;
ekf2.flow_x = RadOfDeg(flow_y) * (1e-6 * ekf2.flow_dt); // INTEGRATED FLOW AROUND Y AXIS (RIGHT -X, LEFT +X)
ekf2.flow_y = - RadOfDeg(flow_x) * (1e-6 * ekf2.flow_dt); // INTEGRATED FLOW AROUND X AXIS (FORWARD +Y, BACKWARD -Y)
ekf2.gyro_roll = NAN;
ekf2.gyro_pitch = NAN;
ekf2.gyro_yaw = NAN;
/* once callback initiated, build the
optical flow message with what is received */
flow_msg.quality = quality; // quality indicator between 0 and 255
flow_msg.flowdata = Vector2f(ekf2.flow_x, ekf2.flow_y); // measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive
flow_msg.gyrodata = Vector3f{ekf2.gyro_roll, ekf2.gyro_pitch, ekf2.gyro_yaw}; // measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
flow_msg.dt = ekf2.flow_dt; // amount of integration time (usec)
// update the optical flow data based on the callback
ekf.setOpticalFlowData(stamp, &flow_msg);
}
sw/airborne/subsystems/ins/ins_ekf2.h
+2
View File
@@ -38,11 +38,13 @@ extern "C" {
struct ekf2_parameters_t { struct ekf2_parameters_t {
int32_t mag_fusion_type; int32_t mag_fusion_type;
int32_t fusion_mode;
}; };
extern void ins_ekf2_init(void); extern void ins_ekf2_init(void);
extern void ins_ekf2_update(void); extern void ins_ekf2_update(void);
extern void ins_ekf2_change_param(int32_t unk); extern void ins_ekf2_change_param(int32_t unk);
extern void ins_ekf2_remove_gps(int32_t mode);
extern struct ekf2_parameters_t ekf2_params; extern struct ekf2_parameters_t ekf2_params;
#ifdef __cplusplus #ifdef __cplusplus
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: fca8d15eff...c6e88ccbb9