fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Optical flow simulator and install cleanup. (#5132)

Browse Source 
sitl CI is having some issue cloning, but I have verified it locally for various configs
This commit is contained in:
James Goppert 2016-07-24 13:07:14 -04:00 committed by GitHub
parent 5cee3fa0e1
commit 05315abc89
11 changed files with 172 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
CMakeLists.txt
View File

@ -152,6 +152,17 @@ set(target_name "${OS}-${BOARD}-${LABEL}")
message(STATUS "${target_name}")
# Define GNU standard installation directories
include(GNUInstallDirs)
# Setup install paths
if(NOT CMAKE_INSTALL_PREFIX)
if (${OS} STREQUAL "posix")
set(CMAKE_INSTALL_PREFIX "/usr" CACHE PATH "Install path prefix" FORCE)
endif()
endif()
message(STATUS "CMAKE_INSTALL_PREFIX: ${CMAKE_INSTALL_PREFIX}")
# switch to ros CMake file if building ros
if (${OS} STREQUAL "ros")
include("cmake/ros-CMakeLists.txt")
@ -202,7 +213,7 @@ if (NOT ${CMAKE_VERSION} VERSION_LESS 3.1.0)
endif()
set(version_major 1)
set(version_minor 0)
set(version_minor 4)
set(version_patch 1)
set(version "${version_major}.${version_minor}.${version_patch}")
set(package-contact "px4users@googlegroups.com")
@ -371,10 +382,27 @@ px4_create_git_hash_header(HEADER ${CMAKE_BINARY_DIR}/build_git_version.h)
#
# Important to having packaging at end of cmake file.
#
set(CPACK_PACKAGE_NAME ${PROJECT_NAME}-${CONFIG})
set(CPACK_PACKAGE_VERSION ${version})
set(CPACK_PACKAGE_CONTACT ${package_contact})
set(CPACK_PACKAGE_CONTACT ${package-contact})
set(CPACK_DEBIAN_PACKAGE_SHLIBDEPS ON)
set(CPACK_DEBIAN_PACKAGE_SECTION "devel")
set(CPACK_DEBIAN_PACKAGE_PRIORITY "optional")
set(short-description "The px4 autopilot.")
set(CPACK_DEBIAN_PACKAGE_DESCRIPTION ${short-description})
set(CPACK_GENERATOR "ZIP")
set(CPACK_SOURCE_GENERATOR "ZIP")
set(CPACK_PACKAGE_FILE_NAME "${PROJECT_NAME}-${CONFIG}-${version}")
set(CPACK_SOURCE_PACKAGE_FILE_NAME "${PROJECT_NAME}-${version}")
set(CPACK_SOURCE_GENERATOR "ZIP;TBZ2")
if ("${CMAKE_SYSTEM}" MATCHES "Linux")
find_program(DPKG_PROGRAM dpkg)
if (EXISTS ${DPKG_PROGRAM})
list (APPEND CPACK_GENERATOR "DEB")
endif()
endif()
if (${OS} STREQUAL "posix")
set(CPACK_SET_DESTDIR "ON")
endif()
include(CPack)
endif() # ros alternative endif

16
Makefile
View File

@ -115,16 +115,16 @@ endif
# --------------------------------------------------------------------
# describe how to build a cmake config
define cmake-build
+@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@$(BUILD_DIR_SUFFIX)/Makefile ]; then rm -rf $(PWD)/build_$@$(BUILD_DIR_SUFFIX); fi
+@if [ ! -e $(PWD)/build_$@$(BUILD_DIR_SUFFIX)/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p $(PWD)/build_$@$(BUILD_DIR_SUFFIX) && cd $(PWD)/build_$@$(BUILD_DIR_SUFFIX) && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1) || (cd .. && rm -rf $(PWD)/build_$@$(BUILD_DIR_SUFFIX)); fi
+@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e ./build_$@$(BUILD_DIR_SUFFIX)/Makefile ]; then rm -rf ./build_$@$(BUILD_DIR_SUFFIX); fi
+@if [ ! -e ./build_$@$(BUILD_DIR_SUFFIX)/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p ./build_$@$(BUILD_DIR_SUFFIX) && cd ./build_$@$(BUILD_DIR_SUFFIX) && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1) || (cd .. && rm -rf ./build_$@$(BUILD_DIR_SUFFIX)); fi
+@Tools/check_submodules.sh
+@(echo "PX4 CONFIG: $@$(BUILD_DIR_SUFFIX)" && cd $(PWD)/build_$@$(BUILD_DIR_SUFFIX) && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
+@(echo "PX4 CONFIG: $@$(BUILD_DIR_SUFFIX)" && cd ./build_$@$(BUILD_DIR_SUFFIX) && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
endef
define cmake-build-other
+@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi
+@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake $(2) -G$(PX4_CMAKE_GENERATOR) || (cd .. && rm -rf $(PWD)/build_$@); fi
+@(cd $(PWD)/build_$@ && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
+@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e ./build_$@/Makefile ]; then rm -rf ./build_$@; fi
+@if [ ! -e ./build_$@/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p ./build_$@ && cd ./build_$@ && cmake $(2) -G$(PX4_CMAKE_GENERATOR) || (cd .. && rm -rf ./build_$@); fi
+@(cd ./build_$@ && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
endef
# create empty targets to avoid msgs for targets passed to cmake
@ -241,7 +241,7 @@ run_sitl_ros: sitl_deprecation
gazebo_build:
@mkdir -p build_gazebo
@if [ ! -e $(PWD)/build_gazebo/CMakeCache.txt ];then cd build_gazebo && cmake -Wno-dev -G$(PX4_CMAKE_GENERATOR) $(PWD)/Tools/sitl_gazebo; fi
@if [ ! -e ./build_gazebo/CMakeCache.txt ];then cd build_gazebo && cmake -Wno-dev -G$(PX4_CMAKE_GENERATOR) ../Tools/sitl_gazebo; fi
@cd build_gazebo && $(PX4_MAKE) $(PX4_MAKE_ARGS)
@cd build_gazebo && $(PX4_MAKE) $(PX4_MAKE_ARGS) sdf
@ -339,7 +339,7 @@ distclean: submodulesclean
@git clean -ff -x -d -e ".project" -e ".cproject"
# targets handled by cmake
cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \
cmake_targets = install test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \
run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim replay \
jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_tailsitter gazebo_iris gazebo_iris_opt_flow gazebo_tailsitter \
gazebo_gdb_standard_vtol gazebo_lldb_standard_vtol gazebo_standard_vtol gazebo_plane gazebo_solo gazebo_typhoon_h480

2
Tools/sitl_gazebo

@ -1 +1 @@
Subproject commit 82b354875040d97a3f598f0dc260ddc9ca4a95b3
Subproject commit 0e3e246f9e00ab2281721b72b0ab934a066148f3

64
posix-configs/SITL/init/rcS_lpe_gazebo_iris
View File

@ -10,34 +10,40 @@ param set CAL_ACC0_ID 1376264
param set CAL_ACC1_ID 1310728
param set CAL_MAG0_ID 196616
param set CAL_GYRO0_XOFF 0.01
param set CAL_ACC0_XOFF 1
param set CAL_ACC0_YOFF 2
param set CAL_ACC0_ZOFF 3
param set CAL_ACC0_XOFF 0.01
param set CAL_ACC0_YOFF -0.01
param set CAL_ACC0_ZOFF 0.01
param set CAL_ACC0_XSCALE 1.01
param set CAL_ACC0_YSCALE 1.01
param set CAL_ACC0_ZSCALE 1.01
param set CAL_ACC1_XOFF 0.01
param set CAL_MAG0_XOFF 0.01
param set MC_ROLLRATE_P 0.3
param set MC_PITCHRATE_P 0.3
param set MC_PITCH_P 5.5
param set MC_ROLL_P 5.5
param set MC_ROLLRATE_I 0.1
param set MC_PITCHRATE_I 0.1
param set MPC_XY_P 0.15
param set MPC_XY_VEL_P 0.05
param set MPC_XY_VEL_D 0.005
param set MPC_XY_FF 0.1
param set SENS_BOARD_ROT 8
param set SENS_BOARD_X_OFF 0.000001
# changes for LPE
param set COM_RC_IN_MODE 1
param set NAV_DLL_ACT 2
param set LPE_BETA_MAX 10000
param set COM_DISARM_LAND 3
param set NAV_ACC_RAD 2.0
param set COM_OF_LOSS_T 5
param set COM_OBL_ACT 2
param set COM_OBL_RC_ACT 0
param set RTL_RETURN_ALT 30.0
param set RTL_DESCEND_ALT 5.0
param set RTL_LAND_DELAY 5
param set MIS_TAKEOFF_ALT 2.5
param set MC_ROLLRATE_P 0.2
param set MC_PITCHRATE_P 0.2
param set MC_PITCH_P 6
param set MC_ROLL_P 6
param set MPC_HOLD_MAX_Z 2.0
param set MPC_HOLD_XY_DZ 0.1
param set MPC_Z_VEL_P 0.6
param set MPC_Z_VEL_I 0.15
param set EKF2_GBIAS_INIT 0.01
param set EKF2_ANGERR_INIT 0.01
replay tryapplyparams
simulator start -s
rgbled start
rgbledsim start
tone_alarm start
gyrosim start
accelsim start
@ -51,24 +57,22 @@ commander start
land_detector start multicopter
navigator start
attitude_estimator_q start
local_position_estimator start
mc_pos_control start
mc_att_control start
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
mavlink start -u 14556 -r 2000000
mavlink start -u 14557 -r 2000000 -m onboard -o 14540
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 80 -s ATTITUDE -u 14556
mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556
mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_w.main.mix
mavlink start -u 14556 -r 4000000
mavlink start -u 14557 -r 4000000 -m onboard -o 14540
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 50 -s ATTITUDE -u 14556
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556
mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
sdlog2 start -r 100 -e -t -a
# start LPE at end, when we know it is ok to init sensors
sleep 5
local_position_estimator start
mavlink boot_complete
replay trystart

48
posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
View File

@ -18,27 +18,38 @@ param set CAL_ACC0_YSCALE 1.01
param set CAL_ACC0_ZSCALE 1.01
param set CAL_ACC1_XOFF 0.01
param set CAL_MAG0_XOFF 0.01
param set SENS_BOARD_ROT 8
param set SENS_BOARD_X_OFF 0.000001
param set COM_RC_IN_MODE 1
param set NAV_DLL_ACT 2
param set COM_DISARM_LAND 3
param set NAV_ACC_RAD 12.0
param set RTL_RETURN_ALT 30.0
param set RTL_DESCEND_ALT 10.0
param set RTL_LAND_DELAY 0
param set MIS_TAKEOFF_ALT 2.5
param set MC_ROLLRATE_P 0.3
param set MC_PITCHRATE_P 0.3
param set MC_PITCH_P 5.5
param set MC_ROLL_P 5.5
param set MC_ROLLRATE_I 0.1
param set MC_PITCHRATE_I 0.1
param set MPC_XY_P 0.15
param set MPC_XY_VEL_P 0.05
param set MPC_XY_VEL_D 0.005
param set MPC_XY_FF 0.1
param set SENS_BOARD_ROT 8
param set SENS_BOARD_X_OFF 0.000001
param set COM_RC_IN_MODE 1
param set NAV_ACC_RAD 2.0
param set RTL_RETURN_ALT 30.0
param set RTL_DESCEND_ALT 10.0
param set MPC_HOLD_MAX_Z 2.0
param set MPC_HOLD_XY_DZ 0.1
param set MPC_Z_VEL_P 0.8
param set MPC_Z_VEL_I 0.15
param set MPC_XY_VEL_P 0.15
param set MPC_XY_VEL_I 0.2
# changes for LPE indoor flight
param set LPE_GPS_ON 0
param set MPC_ALT_MODE 1
param set LPE_T_MAX_GRADE 0
param set MPC_XY_VEL_MAX 2
param set MPC_Z_VEL_MAX 2
param set MPC_XY_P 0.5
param set MIS_TAKEOFF_ALT 2
# changes for LPE
param set COM_RC_IN_MODE 1
param set NAV_DLL_ACT 2
param set LPE_BETA_MAX 10000
replay tryapplyparams
simulator start -s
rgbledsim start
@ -55,13 +66,14 @@ commander start
land_detector start multicopter
navigator start
attitude_estimator_q start
local_position_estimator start
mc_pos_control start
mc_att_control start
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_w.main.mix
mavlink start -u 14556 -r 2000000
mavlink start -u 14557 -r 2000000 -m onboard -o 14540
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 80 -s ATTITUDE -u 14556
mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556
@ -70,9 +82,5 @@ mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
sdlog2 start -r 100 -e -t -a
# start LPE at end, when we know it is ok to init sensors
sleep 5
local_position_estimator start
mavlink boot_complete
replay trystart

34
posix-configs/SITL/init/rcS_lpe_jmavsim_iris
View File

@ -18,21 +18,30 @@ param set CAL_ACC0_YSCALE 1.01
param set CAL_ACC0_ZSCALE 1.01
param set CAL_ACC1_XOFF 0.01
param set CAL_MAG0_XOFF 0.01
param set MC_ROLLRATE_P 0.2
param set MC_PITCHRATE_P 0.2
param set MC_PITCH_P 6
param set MC_ROLL_P 6
param set MPC_XY_P 0.4
param set MPC_XY_VEL_P 0.2
param set MPC_XY_VEL_D 0.005
param set SENS_BOARD_ROT 0
param set SENS_BOARD_X_OFF 0.000001
param set COM_RC_IN_MODE 1
param set NAV_DLL_ACT 2
param set COM_DISARM_LAND 3
param set NAV_ACC_RAD 2.0
param set RTL_RETURN_ALT 30.0
param set RTL_DESCEND_ALT 10.0
param set COM_RC_IN_MODE 1
param set NAV_DLL_ACT 2
param set RTL_LAND_DELAY 0
param set MIS_TAKEOFF_ALT 2.5
param set MC_ROLLRATE_P 0.2
param set MC_PITCHRATE_P 0.2
param set MC_PITCH_P 6
param set MC_ROLL_P 6
param set MPC_HOLD_MAX_Z 2.0
param set MPC_HOLD_XY_DZ 0.1
param set MPC_Z_VEL_MAX 2.0
param set MPC_Z_VEL_P 0.4
param set EKF2_GBIAS_INIT 0.01
param set EKF2_ANGERR_INIT 0.01
replay tryapplyparams
simulator start -s
rgbledsim start
tone_alarm start
@ -41,18 +50,22 @@ accelsim start
barosim start
adcsim start
gpssim start
#gps start -d /dev/ttyACM0 -s
pwm_out_sim mode_pwm
sleep 1
sensors start
commander start
land_detector start multicopter
navigator start
attitude_estimator_q start
local_position_estimator start
mc_pos_control start
mc_att_control start
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_x.main.mix
mavlink start -u 14556 -r 2000000
mavlink start -u 14557 -r 2000000 -m onboard -o 14540
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 80 -s ATTITUDE -u 14556
mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556
@ -60,7 +73,8 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
mavlink stream -r 20 -s MANUAL_CONTROL -u 14556
sdlog2 start -r 100 -e -t -a
attitude_estimator_q start
local_position_estimator start
logger start -e -t
mavlink boot_complete
replay trystart

8
src/firmware/posix/CMakeLists.txt
View File

@ -154,4 +154,12 @@ foreach(viewer none jmavsim gazebo replay)
endforeach()
endforeach()
#=============================================================================
# install
#
install(TARGETS mainapp DESTINATION bin)
install(DIRECTORY ${PROJECT_SOURCE_DIR}/ROMFS DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME})
install(DIRECTORY ${PROJECT_SOURCE_DIR}/posix-configs DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME})
# vim: set noet ft=cmake fenc=utf-8 ff=unix :

4
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

@ -25,8 +25,8 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// subscriptions, set rate, add to list
_sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
_sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()),
// flow 10 hz
_sub_flow(ORB_ID(optical_flow), 1000 / 10, 0, &getSubscriptions()),
// set flow max update rate higher than expected to we don't lose packets
_sub_flow(ORB_ID(optical_flow), 1000 / 100, 0, &getSubscriptions()),
// main prediction loop, 100 hz
_sub_sensor(ORB_ID(sensor_combined), 1000 / 100, 0, &getSubscriptions()),
// status updates 2 hz

3
src/modules/local_position_estimator/sensors/flow.cpp
View File

@ -82,6 +82,9 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
float gyro_y_rad = _flow_gyro_y_high_pass.update(
_sub_flow.get().gyro_y_rate_integral);
//warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f",
//double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d));
// compute velocities in camera frame using ground distance
// assume camera frame is body frame
Vector3f delta_b(

4
src/modules/simulator/simulator.h
View File

@ -55,6 +55,7 @@
#include <systemlib/battery.h>
#include <uORB/uORB.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h>
#include <v1.0/mavlink_types.h>
#include <v1.0/common/mavlink.h>
namespace simulator
@ -238,6 +239,7 @@ private:
_gyro_pub(nullptr),
_mag_pub(nullptr),
_flow_pub(nullptr),
_dist_pub(nullptr),
_battery_pub(nullptr),
_initialized(false)
#ifndef __PX4_QURT
@ -288,6 +290,7 @@ private:
orb_advert_t _gyro_pub;
orb_advert_t _mag_pub;
orb_advert_t _flow_pub;
orb_advert_t _dist_pub;
orb_advert_t _battery_pub;
bool _initialized;
@ -298,6 +301,7 @@ private:
// class methods
int publish_sensor_topics(mavlink_hil_sensor_t *imu);
int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
int publish_distance_topic(mavlink_distance_sensor_t *dist);
#ifndef __PX4_QURT
// uORB publisher handlers

30
src/modules/simulator/simulator_mavlink.cpp
View File

@ -283,6 +283,12 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
publish_flow_topic(&flow);
break;
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
mavlink_distance_sensor_t dist;
mavlink_msg_distance_sensor_decode(msg, &dist);
publish_distance_topic(&dist);
break;
case MAVLINK_MSG_ID_HIL_GPS:
mavlink_hil_gps_t gps_sim;
mavlink_msg_hil_gps_decode(msg, &gps_sim);
@ -863,7 +869,7 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro;
flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro;
flow.pixel_flow_x_integral = flow_mavlink->integrated_x;
flow.pixel_flow_x_integral = flow_mavlink->integrated_y;
flow.pixel_flow_y_integral = flow_mavlink->integrated_y;
flow.quality = flow_mavlink->quality;
int flow_multi;
@ -871,3 +877,25 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
return OK;
}
int Simulator::publish_distance_topic(mavlink_distance_sensor_t *dist_mavlink)
{
uint64_t timestamp = hrt_absolute_time();
struct distance_sensor_s dist;
memset(&dist, 0, sizeof(dist));
dist.timestamp = timestamp;
dist.min_distance = dist_mavlink->min_distance / 100.0f;
dist.max_distance = dist_mavlink->max_distance / 100.0f;
dist.current_distance = dist_mavlink->current_distance / 100.0f;
dist.type = dist_mavlink->type;
dist.id = dist_mavlink->id;
dist.orientation = dist_mavlink->orientation;
dist.covariance = dist_mavlink->covariance / 100.0f;
int dist_multi;
orb_publish_auto(ORB_ID(distance_sensor), &_dist_pub, &dist, &dist_multi, ORB_PRIO_HIGH);
return OK;
}