diff --git a/CMakeLists.txt b/CMakeLists.txt index 732496778f..9486bf13fb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -236,6 +236,19 @@ add_custom_target(submodule_clean COMMAND rm -rf .git/modules/* ) +#============================================================================= +# misc targets +# +add_custom_target(check_format + COMMAND Tools/check_code_style.sh + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + +add_custom_target(config + COMMAND cmake-gui . + WORKING_DIRECTORY ${CMAKE_BINARY_DIR} + ) + #============================================================================= # external libraries # diff --git a/Makefile b/Makefile index c0413ea7ef..7cba819317 100644 --- a/Makefile +++ b/Makefile @@ -113,12 +113,18 @@ px4fmu-v2_default: px4fmu-v2_simple: $(call cmake-build,nuttx_px4fmu-v2_simple) +px4fmu-v2_lpe: + $(call cmake-build,nuttx_px4fmu-v2_lpe) + nuttx_sim_simple: $(call cmake-build,$@) posix_sitl_simple: $(call cmake-build,$@) +posix_sitl_lpe: + $(call cmake-build,$@) + ros_sitl_simple: $(call cmake-build,$@) @@ -134,44 +140,13 @@ posix_sitl_default: posix_sitl_simple ros: ros_sitl_simple -run_sitl_quad: posix - Tools/sitl_run.sh posix-configs/SITL/init/rcS none jmavsim - -run_sitl_iris: posix - Tools/sitl_run.sh posix-configs/SITL/init/rcS_iris_gazebo none gazebo - -run_sitl_plane: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing - -run_sitl_ros: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc_iris_ros - -lldb_sitl_quad: posix - Tools/sitl_run.sh posix-configs/SITL/init/rcS lldb jmavsim - -lldb_sitl_plane: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing lldb - -lldb_sitl_ros: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc_iris_ros lldb - -gdb_sitl_quad: posix - Tools/sitl_run.sh posix-configs/SITL/init/rcS gdb jmavsim - -gdb_sitl_plane: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing lldb - -gdb_sitl_ros: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc_iris_ros lldb - -sitl_quad: - @echo "Deprecated. Use 'run_sitl_quad' instead." - -sitl_plane: - @echo "Deprecated. Use 'run_sitl_plane' instead." +sitl_deprecation: + @echo "Deprecated. Use 'make posix_sitl_default run_sitl' instead." + @echo "Change init script with 'make posix_sitl_default config'" -sitl_ros: - @echo "Deprecated. Use 'run_sitl_ros' instead." +sitl_quad: sitl_deprecation +sitl_plane: sitl_deprecation +sitl_ros: sitl_deprecation # Other targets # -------------------------------------------------------------------- @@ -184,7 +159,8 @@ clean: @(cd src/modules/uavcan/libuavcan && git clean -d -f -x) # 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 libuavcan +cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \ + run_sitl config $(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ)))) .PHONY: clean diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 1dfe6c09bc..4c6cdbd7cd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -12,7 +12,13 @@ then attitude_estimator_q start position_estimator_inav start else - ekf_att_pos_estimator start + if param compare LPE_ENABLED 1 + then + attitude_estimator_q start + local_position_estimator start + else + ekf_att_pos_estimator start + fi fi if mc_att_control start diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index 06c6bea1cc..c3dd2a3f32 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -26,6 +26,7 @@ for fn in $(find src/examples \ src/modules/dataman \ src/modules/fixedwing_backside \ src/modules/segway \ + src/modules/local_position_estimator \ src/modules/unit_test \ src/modules/systemlib \ src/modules/controllib \ diff --git a/Tools/posix.gdbinit b/Tools/posix.gdbinit index 0de1123362..23fa44b4e0 100644 --- a/Tools/posix.gdbinit +++ b/Tools/posix.gdbinit @@ -1,2 +1,2 @@ -handle SIGCONT nostop +handle SIGCONT nostop noprint nopass run diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index d80fcc2c70..3f7dc1a883 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -1,10 +1,28 @@ #!/bin/bash -cp Tools/posix_lldbinit build_posix_sitl_simple/src/firmware/posix/.lldbinit -cp Tools/posix.gdbinit build_posix_sitl_simple/src/firmware/posix/.gdbinit + +rc_script=$1 +debugger=$2 +program=$3 +build_path=$4 + +echo SITL ARGS +echo rc_script: $rc_script +echo debugger: $debugger +echo program: $program +echo build_path: $buid_path + +if [ "$#" != 4 ] +then + echo usage: sitl_run.sh rc_script debugger program build_path + exit 1 +fi + +cp Tools/posix_lldbinit $build_path/src/firmware/posix/.lldbinit +cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit SIM_PID=0 -if [ "$3" == "jmavsim" ] +if [ "$program" == "jmavsim" ] then cd Tools/jMAVSim ant @@ -22,19 +40,19 @@ then exit 1 fi fi -cd build_posix_sitl_simple/src/firmware/posix +cd $build_path/src/firmware/posix mkdir -p rootfs/fs/microsd mkdir -p rootfs/eeprom touch rootfs/eeprom/parameters # Start Java simulator -if [ "$2" == "lldb" ] +if [ "$debugger" == "lldb" ] then - lldb -- mainapp ../../../../$1 -elif [ "$2" == "gdb" ] + lldb -- mainapp ../../../../$rc_script +elif [ "$debugger" == "gdb" ] then - gdb --args mainapp ../../../../$1 + gdb --args mainapp ../../../../$rc_script else - ./mainapp ../../../../$1 + ./mainapp ../../../../$rc_script fi if [ "$3" == "jmavsim" ] diff --git a/cmake/configs/nuttx_px4fmu-v2_lpe.cmake b/cmake/configs/nuttx_px4fmu-v2_lpe.cmake new file mode 100644 index 0000000000..6e5cd10ba4 --- /dev/null +++ b/cmake/configs/nuttx_px4fmu-v2_lpe.cmake @@ -0,0 +1,5 @@ +include(cmake/configs/nuttx_px4fmu-v2_default.cmake) + +list(APPEND config_module_list + modules/local_position_estimator + ) diff --git a/cmake/configs/posix_sitl_lpe.cmake b/cmake/configs/posix_sitl_lpe.cmake new file mode 100644 index 0000000000..0598044844 --- /dev/null +++ b/cmake/configs/posix_sitl_lpe.cmake @@ -0,0 +1,9 @@ +include(cmake/configs/posix_sitl_simple.cmake) + +list(APPEND config_module_list + modules/local_position_estimator + ) + +set(config_sitl_rcS + posix-configs/SITL/init/rcS_lpe + ) diff --git a/cmake/configs/posix_sitl_simple.cmake b/cmake/configs/posix_sitl_simple.cmake index 227bffeab8..b0a8158070 100644 --- a/cmake/configs/posix_sitl_simple.cmake +++ b/cmake/configs/posix_sitl_simple.cmake @@ -62,6 +62,27 @@ set(config_extra_builtin_cmds sercon ) +set(config_sitl_rcS + posix-configs/SITL/init/rcS + CACHE FILEPATH "init script for sitl" + ) + +set(config_sitl_viewer + jmavsim + CACHE STRING "viewer for sitl" + ) +set_property(CACHE config_sitl_viewer + PROPERTY STRINGS "jmavsim;none") + +set(config_sitl_debugger + disable + CACHE STRING "debugger for sitl" + ) +set_property(CACHE config_sitl_debugger + PROPERTY STRINGS "disable;gdb;lldb") + + + add_custom_target(sercon) set_target_properties(sercon PROPERTIES MAIN "sercon" STACK "2048") diff --git a/posix-configs/SITL/init/rcS_lpe b/posix-configs/SITL/init/rcS_lpe new file mode 100644 index 0000000000..9b416a0b26 --- /dev/null +++ b/posix-configs/SITL/init/rcS_lpe @@ -0,0 +1,65 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 2 +param set MC_PITCHRATE_P 0.15 +param set MC_ROLLRATE_P 0.15 +param set MC_YAW_P 2.0 +param set MC_YAWRATE_P 0.35 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_XOFF 0.01 +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 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 + +# changes for LPE +param set COM_RC_IN_MODE 1 +param set LPE_BETA_MAX 10000 + +rgbled start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +attitude_estimator_q start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix +mavlink start -u 14556 -r 2000000 +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_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 boot_complete +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 diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt index 4632383bff..be6f9b1062 100644 --- a/src/firmware/nuttx/CMakeLists.txt +++ b/src/firmware/nuttx/CMakeLists.txt @@ -5,13 +5,6 @@ px4_nuttx_generate_builtin_commands( ${config_extra_builtin_cmds} ) -px4_nuttx_add_romfs(OUT romfs - ROOT ROMFS/px4fmu_common - EXTRAS ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin - ) - -add_dependencies(romfs fw_io) - # add executable add_executable(firmware_nuttx builtin_commands.c) @@ -33,8 +26,6 @@ if(NOT ${BOARD} STREQUAL "sim") set_target_properties(firmware_nuttx PROPERTIES LINK_FLAGS ${main_link_flags}) endif() -set(fw_file ${CMAKE_CURRENT_BINARY_DIR}/${target_name}.px4) - target_link_libraries(firmware_nuttx -Wl,--start-group ${module_libraries} @@ -49,6 +40,14 @@ add_custom_target(check_weak ) if(NOT ${BOARD} STREQUAL "sim") + + + px4_nuttx_add_romfs(OUT romfs + ROOT ROMFS/px4fmu_common + EXTRAS ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin + ) + add_dependencies(romfs fw_io) + set(fw_file ${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4) diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 7cef387962..e4c2855c66 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -24,4 +24,11 @@ else() ) endif() +add_custom_target(run_sitl + COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" "${config_sitl_debugger}" + "${config_sitl_viewer}" "${CMAKE_BINARY_DIR}" + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) +add_dependencies(run_sitl mainapp) + # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 1aa60e63c8..04b96753ef 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -539,6 +539,7 @@ int blockRandGaussTest() } float stdDev = sqrt(sum / (n - 1)); + (void)(stdDev); ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1)); ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1)); printf("PASS\n"); diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp new file mode 100644 index 0000000000..08cf51fb5c --- /dev/null +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -0,0 +1,1379 @@ +#include "BlockLocalPositionEstimator.hpp" +#include +#include +#include + +static const int MIN_FLOW_QUALITY = 100; +static const int REQ_INIT_COUNT = 100; + +static const uint32_t VISION_POSITION_TIMEOUT = 500000; +static const uint32_t MOCAP_TIMEOUT = 200000; + +static const uint32_t XY_SRC_TIMEOUT = 2000000; + +using namespace std; + +BlockLocalPositionEstimator::BlockLocalPositionEstimator() : + // this block has no parent, and has name LPE + SuperBlock(NULL, "LPE"), + + // subscriptions, set rate, add to list + // TODO topic speed limiting? + _sub_status(ORB_ID(vehicle_status), 0, 0, &getSubscriptions()), + _sub_armed(ORB_ID(actuator_armed), 0, 0, &getSubscriptions()), + _sub_control_mode(ORB_ID(vehicle_control_mode), + 0, 0, &getSubscriptions()), + _sub_att(ORB_ID(vehicle_attitude), 0, 0, &getSubscriptions()), + _sub_att_sp(ORB_ID(vehicle_attitude_setpoint), + 0, 0, &getSubscriptions()), + _sub_flow(ORB_ID(optical_flow), 0, 0, &getSubscriptions()), + _sub_sensor(ORB_ID(sensor_combined), 0, 0, &getSubscriptions()), + _sub_distance(ORB_ID(distance_sensor), + 0, 0, &getSubscriptions()), + _sub_param_update(ORB_ID(parameter_update), 0, 0, &getSubscriptions()), + _sub_manual(ORB_ID(manual_control_setpoint), 0, 0, &getSubscriptions()), + _sub_home(ORB_ID(home_position), 0, 0, &getSubscriptions()), + _sub_gps(ORB_ID(vehicle_gps_position), 0, 0, &getSubscriptions()), + _sub_vision_pos(ORB_ID(vision_position_estimate), 0, 0, &getSubscriptions()), + _sub_mocap(ORB_ID(att_pos_mocap), 0, 0, &getSubscriptions()), + + // publications + _pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()), + _pub_gpos(ORB_ID(vehicle_global_position), -1, &getPublications()), + _pub_filtered_flow(ORB_ID(filtered_bottom_flow), -1, &getPublications()), + _pub_est_status(ORB_ID(estimator_status), -1, &getPublications()), + + // map projection + _map_ref(), + + // block parameters + _integrate(this, "INTEGRATE"), + _flow_xy_stddev(this, "FLW_XY"), + _sonar_z_stddev(this, "SNR_Z"), + _lidar_z_stddev(this, "LDR_Z"), + _accel_xy_stddev(this, "ACC_XY"), + _accel_z_stddev(this, "ACC_Z"), + _baro_stddev(this, "BAR_Z"), + _gps_xy_stddev(this, "GPS_XY"), + _gps_z_stddev(this, "GPS_Z"), + _gps_vxy_stddev(this, "GPS_VXY"), + _gps_vz_stddev(this, "GPS_VZ"), + _gps_eph_max(this, "EPH_MAX"), + _vision_xy_stddev(this, "VIS_XY"), + _vision_z_stddev(this, "VIS_Z"), + _no_vision(this, "NO_VISION"), + _beta_max(this, "BETA_MAX"), + _mocap_p_stddev(this, "VIC_P"), + _pn_p_noise_power(this, "PN_P"), + _pn_v_noise_power(this, "PN_V"), + _pn_b_noise_power(this, "PN_B"), + + // misc + _polls(), + _timeStamp(hrt_absolute_time()), + _time_last_xy(0), + _time_last_flow(0), + _time_last_baro(0), + _time_last_gps(0), + _time_last_lidar(0), + _time_last_sonar(0), + _time_last_vision_p(0), + _time_last_mocap(0), + + // mavlink log + _mavlink_fd(open(MAVLINK_LOG_DEVICE, 0)), + + // initialization flags + _baroInitialized(false), + _gpsInitialized(false), + _lidarInitialized(false), + _sonarInitialized(false), + _flowInitialized(false), + _visionInitialized(false), + _mocapInitialized(false), + + // init counts + _baroInitCount(0), + _gpsInitCount(0), + _lidarInitCount(0), + _sonarInitCount(0), + _flowInitCount(0), + _visionInitCount(0), + _mocapInitCount(0), + + // reference altitudes + _altHome(0), + _altHomeInitialized(false), + _baroAltHome(0), + _gpsAltHome(0), + _lidarAltHome(0), + _sonarAltHome(0), + _visionHome(), + _mocapHome(), + + // flow integration + _flowX(0), + _flowY(0), + _flowMeanQual(0), + + // reference lat/lon + _gpsLatHome(0), + _gpsLonHome(0), + + // status + _canEstimateXY(false), + _canEstimateZ(false), + _xyTimeout(false), + + // faults + _baroFault(FAULT_NONE), + _gpsFault(FAULT_NONE), + _lidarFault(FAULT_NONE), + _flowFault(FAULT_NONE), + _sonarFault(FAULT_NONE), + _visionFault(FAULT_NONE), + _mocapFault(FAULT_NONE), + + //timeouts + _visionTimeout(true), + _mocapTimeout(true), + + // loop performance + _loop_perf(), + _interval_perf(), + _err_perf(), + + // kf matrices + _x(), _u(), _P() +{ + // setup event triggering based on new flow messages to integrate + _polls[POLL_FLOW].fd = _sub_flow.getHandle(); + _polls[POLL_FLOW].events = POLLIN; + + _polls[POLL_PARAM].fd = _sub_param_update.getHandle(); + _polls[POLL_PARAM].events = POLLIN; + + _polls[POLL_SENSORS].fd = _sub_sensor.getHandle(); + _polls[POLL_SENSORS].events = POLLIN; + + // initialize P to identity*0.1 + initP(); + + _x.setZero(); + _u.setZero(); + + // perf counters + _loop_perf = perf_alloc(PC_ELAPSED, + "local_position_estimator_runtime"); + //_interval_perf = perf_alloc(PC_INTERVAL, + //"local_position_estimator_interval"); + _err_perf = perf_alloc(PC_COUNT, "local_position_estimator_err"); + + // map + _map_ref.init_done = false; + + // intialize parameter dependent matrices + updateParams(); +} + +BlockLocalPositionEstimator::~BlockLocalPositionEstimator() +{ +} + +void BlockLocalPositionEstimator::update() +{ + + // wait for a sensor update, check for exit condition every 100 ms + int ret = poll(_polls, 3, 100); + + if (ret < 0) { + /* poll error, count it in perf */ + perf_count(_err_perf); + return; + } + + uint64_t newTimeStamp = hrt_absolute_time(); + float dt = (newTimeStamp - _timeStamp) / 1.0e6f; + _timeStamp = newTimeStamp; + + //printf("dt: %0.5g\n", double(dt)); + + // set dt for all child blocks + setDt(dt); + + // see which updates are available + bool flowUpdated = _sub_flow.updated(); + bool paramsUpdated = _sub_param_update.updated(); + bool baroUpdated = _sub_sensor.updated(); + bool lidarUpdated = false; + bool sonarUpdated = false; + + if (_sub_distance.updated()) { + if (_sub_distance.get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER) { + lidarUpdated = true; + } + + if (_sub_distance.get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND) { + sonarUpdated = true; + } + + if (_sub_distance.get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_INFRARED) { + mavlink_log_info(_mavlink_fd, "[lpe] no support to short-range infrared sensors "); + warnx("[lpe] short-range infrared detected. Ignored... "); + } + } + + bool gpsUpdated = _sub_gps.updated(); + bool homeUpdated = _sub_home.updated(); + bool visionUpdated = _sub_vision_pos.updated(); + bool mocapUpdated = _sub_mocap.updated(); + + // get new data + updateSubscriptions(); + + // update parameters + if (paramsUpdated) { + updateParams(); + } + + // update home position projection + if (homeUpdated) { + updateHome(); + } + + // check for timeouts on external sources + if ((hrt_absolute_time() - _time_last_vision_p > VISION_POSITION_TIMEOUT) && _visionInitialized) { + if (!_visionTimeout) { + _visionTimeout = true; + mavlink_log_info(_mavlink_fd, "[lpe] vision position timeout "); + warnx("[lpe] vision position timeout "); + } + + } else { + _visionTimeout = false; + } + + if ((hrt_absolute_time() - _time_last_mocap > MOCAP_TIMEOUT) && _mocapInitialized) { + if (!_mocapTimeout) { + _mocapTimeout = true; + mavlink_log_info(_mavlink_fd, "[lpe] mocap timeout "); + warnx("[lpe] mocap timeout "); + } + + } else { + _mocapTimeout = false; + } + + // determine if we should start estimating + _canEstimateZ = _baroInitialized && !_baroFault; + _canEstimateXY = + (_gpsInitialized && !_gpsFault) || + (_flowInitialized && !_flowFault) || + (_visionInitialized && !_visionTimeout && !_visionFault) || + (_mocapInitialized && !_mocapTimeout && !_mocapFault); + + if (_canEstimateXY) { + _time_last_xy = hrt_absolute_time(); + } + + // if we have no lat, lon initialized projection at 0,0 + if (_canEstimateXY && !_map_ref.init_done) { + map_projection_init(&_map_ref, 0, 0); + } + + // reinitialize x if necessary + bool reinit_x = false; + + for (int i = 0; i < n_x; i++) { + // should we do a reinit + // of sensors here? + // don't want it to take too long + if (!isfinite(_x(i))) { + reinit_x = true; + break; + } + } + + if (reinit_x) { + for (int i = 0; i < n_x; i++) { + _x(i) = 0; + } + + mavlink_log_info(_mavlink_fd, "[lpe] reinit x"); + warnx("[lpe] reinit x"); + } + + // reinitialize P if necessary + bool reinit_P = false; + + for (int i = 0; i < n_x; i++) { + for (int j = 0; j < n_x; j++) { + if (!isfinite(_P(i, j))) { + reinit_P = true; + break; + } + } + + if (reinit_P) { break; } + } + + if (reinit_P) { + mavlink_log_info(_mavlink_fd, "[lpe] reinit P"); + warnx("[lpe] reinit P"); + initP(); + } + + // do prediction + predict(); + + // sensor corrections/ initializations + if (gpsUpdated) { + if (!_gpsInitialized) { + initGps(); + + } else { + correctGps(); + } + } + + if (baroUpdated) { + if (!_baroInitialized) { + initBaro(); + + } else { + correctBaro(); + } + } + + if (lidarUpdated) { + if (!_lidarInitialized) { + initLidar(); + + } else { + correctLidar(); + } + } + + if (sonarUpdated) { + if (!_sonarInitialized) { + initSonar(); + + } else { + correctSonar(); + } + } + + if (flowUpdated) { + if (!_flowInitialized) { + initFlow(); + + } else { + perf_begin(_loop_perf);// TODO + correctFlow(); + //perf_count(_interval_perf); + perf_end(_loop_perf); + } + } + + if (_no_vision.get() != CBRK_NO_VISION_KEY) { // check if no vision circuit breaker is set + if (visionUpdated) { + if (!_visionInitialized) { + initVision(); + + } else { + correctVision(); + } + } + } + + if (mocapUpdated) { + if (!_mocapInitialized) { + initmocap(); + + } else { + correctmocap(); + } + } + + _xyTimeout = (hrt_absolute_time() - _time_last_xy > XY_SRC_TIMEOUT); + + if (!_xyTimeout && _altHomeInitialized) { + // update all publications if possible + publishLocalPos(); + publishEstimatorStatus(); + publishGlobalPos(); + publishFilteredFlow(); + + } else if (_altHomeInitialized) { + // publish only Z estimate + publishLocalPos(); + publishEstimatorStatus(); + } + +} + +void BlockLocalPositionEstimator::updateHome() +{ + double lat = _sub_home.get().lat; + double lon = _sub_home.get().lon; + float alt = _sub_home.get().alt; + + mavlink_log_info(_mavlink_fd, "[lpe] home: lat %5.0f, lon %5.0f, alt %5.0f", lat, lon, double(alt)); + warnx("[lpe] home: lat %5.0f, lon %5.0f, alt %5.0f", lat, lon, double(alt)); + map_projection_init(&_map_ref, lat, lon); + float delta_alt = alt - _altHome; + _altHomeInitialized = true; + _altHome = alt; + _gpsAltHome += delta_alt; + _baroAltHome += delta_alt; + _lidarAltHome += delta_alt; + _sonarAltHome += delta_alt; +} + +void BlockLocalPositionEstimator::initBaro() +{ + // collect baro data + if (!_baroInitialized && + (_sub_sensor.get().baro_timestamp[0] != _time_last_baro)) { + _time_last_baro = _sub_sensor.get().baro_timestamp[0]; + _baroAltHome += _sub_sensor.get().baro_alt_meter[0]; + + if (_baroInitCount++ > REQ_INIT_COUNT) { + _baroAltHome /= _baroInitCount; + mavlink_log_info(_mavlink_fd, + "[lpe] baro offs: %d m", (int)_baroAltHome); + warnx("[lpe] baro offs: %d m", (int)_baroAltHome); + _baroInitialized = true; + + if (!_altHomeInitialized) { + _altHomeInitialized = true; + _altHome = _baroAltHome; + } + } + } +} + + +void BlockLocalPositionEstimator::initGps() +{ + // collect gps data + if (!_gpsInitialized && _sub_gps.get().fix_type > 2) { + double lat = _sub_gps.get().lat * 1e-7; + double lon = _sub_gps.get().lon * 1e-7; + float alt = _sub_gps.get().alt * 1e-3f; + // increament sums for mean + _gpsLatHome += lat; + _gpsLonHome += lon; + _gpsAltHome += alt; + _time_last_gps = _sub_gps.get().timestamp_position; + + if (_gpsInitCount++ > REQ_INIT_COUNT) { + _gpsLatHome /= _gpsInitCount; + _gpsLonHome /= _gpsInitCount; + _gpsAltHome /= _gpsInitCount; + map_projection_init(&_map_ref, lat, lon); + mavlink_log_info(_mavlink_fd, "[lpe] gps init: " + "lat %d, lon %d, alt %d m", + int(_gpsLatHome), int(_gpsLonHome), int(_gpsAltHome)); + warnx("[lpe] gps init: lat %d, lon %d, alt %d m", + int(_gpsLatHome), int(_gpsLonHome), int(_gpsAltHome)); + _gpsInitialized = true; + + if (!_altHomeInitialized) { + _altHomeInitialized = true; + _altHome = _gpsAltHome; + } + } + } +} + +void BlockLocalPositionEstimator::initLidar() +{ + + if (_sub_distance.get().type != distance_sensor_s::MAV_DISTANCE_SENSOR_LASER) { return; } + + // collect lidar data + bool valid = false; + float d = _sub_distance.get().current_distance; + + if (d < _sub_distance.get().max_distance && + d > _sub_distance.get().min_distance) { + valid = true; + } + + if (!_lidarInitialized && valid) { + // increament sums for mean + _lidarAltHome += _sub_distance.get().current_distance; + + if (_lidarInitCount++ > REQ_INIT_COUNT) { + _lidarAltHome /= _lidarInitCount; + mavlink_log_info(_mavlink_fd, "[lpe] lidar init: " + "alt %d cm", + int(100 * _lidarAltHome)); + warnx("[lpe] lidar init: alt %d cm", + int(100 * _lidarAltHome)); + _lidarInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::initSonar() +{ + + if (_sub_distance.get().type != distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND) { return; } + + // collect sonar data + bool valid = false; + float d = _sub_distance.get().current_distance; + + if (d < _sub_distance.get().max_distance && + d > _sub_distance.get().min_distance) { + valid = true; + } + + if (!_sonarInitialized && valid) { + // increament sums for mean + _sonarAltHome += _sub_distance.get().current_distance; + + if (_sonarInitCount++ > REQ_INIT_COUNT) { + _sonarAltHome /= _sonarInitCount; + mavlink_log_info(_mavlink_fd, "[lpe] sonar init: " + "alt %d cm", + int(100 * _sonarAltHome)); + warnx("[lpe] sonar init: alt %d cm", + int(100 * _sonarAltHome)); + _sonarInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::initFlow() +{ + + // collect pixel flow data + if (!_flowInitialized) { + // increament sums for mean + _flowMeanQual += _sub_flow.get().quality; + + if (_flowInitCount++ > REQ_INIT_COUNT) { + _flowMeanQual /= _flowInitCount; + + if (_flowMeanQual < MIN_FLOW_QUALITY) { + // retry initialisation till we have better flow data + warnx("[lpe] flow quality bad, retrying init : %d", + int(_flowMeanQual)); + _flowMeanQual = 0; + _flowInitCount = 0; + return; + } + + mavlink_log_info(_mavlink_fd, "[lpe] flow init: " + "quality %d", + int(_flowMeanQual)); + warnx("[lpe] flow init: quality %d", + int(_flowMeanQual)); + _flowInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::initVision() +{ + // collect vision position data + if (!_visionInitialized) { + // increament sums for mean + Vector3f pos; + pos(0) = _sub_vision_pos.get().x; + pos(1) = _sub_vision_pos.get().y; + pos(2) = _sub_vision_pos.get().z; + _visionHome += pos; + + if (_visionInitCount++ > REQ_INIT_COUNT) { + _visionHome /= _visionInitCount; + mavlink_log_info(_mavlink_fd, "[lpe] vision position init: " + "%f, %f, %f m", double(pos(0)), double(pos(1)), double(pos(2))); + warnx("[lpe] vision position init: " + "%f, %f, %f m", double(pos(0)), double(pos(1)), double(pos(2))); + _visionInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::initmocap() +{ + // collect mocap data + if (!_mocapInitialized) { + // increament sums for mean + Vector3f pos; + pos(0) = _sub_mocap.get().x; + pos(1) = _sub_mocap.get().y; + pos(2) = _sub_mocap.get().z; + _mocapHome += pos; + + if (_mocapInitCount++ > REQ_INIT_COUNT) { + _mocapHome /= _mocapInitCount; + mavlink_log_info(_mavlink_fd, "[lpe] mocap init: " + "%f, %f, %f m", double(pos(0)), double(pos(1)), double(pos(2))); + warnx("[lpe] mocap init: " + "%f, %f, %f m", double(pos(0)), double(pos(1)), double(pos(2))); + _mocapInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::publishLocalPos() +{ + // publish local position + if (isfinite(_x(X_x)) && isfinite(_x(X_y)) && isfinite(_x(X_z)) && + isfinite(_x(X_vx)) && isfinite(_x(X_vy)) + && isfinite(_x(X_vz))) { + _pub_lpos.get().timestamp = _timeStamp; + _pub_lpos.get().xy_valid = _canEstimateXY; + _pub_lpos.get().z_valid = _canEstimateZ; + _pub_lpos.get().v_xy_valid = _canEstimateXY; + _pub_lpos.get().v_z_valid = _canEstimateZ; + _pub_lpos.get().x = _x(X_x); // north + _pub_lpos.get().y = _x(X_y); // east + _pub_lpos.get().z = _x(X_z); // down + _pub_lpos.get().vx = _x(X_vx); // north + _pub_lpos.get().vy = _x(X_vy); // east + _pub_lpos.get().vz = _x(X_vz); // down + _pub_lpos.get().yaw = _sub_att.get().yaw; + _pub_lpos.get().xy_global = _sub_home.get().timestamp != 0; // need home for reference + _pub_lpos.get().z_global = _baroInitialized; + _pub_lpos.get().ref_timestamp = _sub_home.get().timestamp; + _pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI; + _pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI; + _pub_lpos.get().ref_alt = _sub_home.get().alt; + // TODO, terrain alt + _pub_lpos.get().dist_bottom = -_x(X_z); + _pub_lpos.get().dist_bottom_rate = -_x(X_vz); + _pub_lpos.get().surface_bottom_timestamp = 0; + _pub_lpos.get().dist_bottom_valid = true; + _pub_lpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); + _pub_lpos.get().epv = sqrtf(_P(X_z, X_z)); + _pub_lpos.update(); + } +} + +void BlockLocalPositionEstimator::publishEstimatorStatus() +{ + if (isfinite(_x(X_x)) && isfinite(_x(X_y)) && isfinite(_x(X_z)) && + isfinite(_x(X_vx)) && isfinite(_x(X_vy)) + && isfinite(_x(X_vz))) { + _pub_est_status.get().timestamp = _timeStamp; + + for (int i = 0; i < n_x; i++) { + _pub_est_status.get().states[i] = _x(i); + _pub_est_status.get().covariances[i] = _P(i, i); + } + + _pub_est_status.get().n_states = n_x; + _pub_est_status.get().nan_flags = 0; + _pub_est_status.get().health_flags = + ((_baroFault > 0) << SENSOR_BARO) + + ((_gpsFault > 0) << SENSOR_GPS) + + ((_lidarFault > 0) << SENSOR_LIDAR) + + ((_flowFault > 0) << SENSOR_FLOW) + + ((_sonarFault > 0) << SENSOR_SONAR) + + ((_visionFault > 0) << SENSOR_VISION) + + ((_mocapFault > 0) << SENSOR_MOCAP); + _pub_est_status.get().timeout_flags = + (_xyTimeout << 0) + + (_visionTimeout << 1) + + (_mocapTimeout << 2); + _pub_est_status.update(); + } +} + +void BlockLocalPositionEstimator::publishGlobalPos() +{ + // publish global position + double lat = 0; + double lon = 0; + map_projection_reproject(&_map_ref, _x(X_x), _x(X_y), &lat, &lon); + float alt = -_x(X_z) + _altHome; + + if (isfinite(lat) && isfinite(lon) && isfinite(alt) && + isfinite(_x(X_vx)) && isfinite(_x(X_vy)) && + isfinite(_x(X_vz))) { + _pub_gpos.get().timestamp = _timeStamp; + _pub_gpos.get().time_utc_usec = _sub_gps.get().time_utc_usec; + _pub_gpos.get().lat = lat; + _pub_gpos.get().lon = lon; + _pub_gpos.get().alt = alt; + _pub_gpos.get().vel_n = _x(X_vx); + _pub_gpos.get().vel_e = _x(X_vy); + _pub_gpos.get().vel_d = _x(X_vz); + _pub_gpos.get().yaw = _sub_att.get().yaw; + _pub_gpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); + _pub_gpos.get().epv = sqrtf(_P(X_z, X_z)); + _pub_gpos.get().terrain_alt = 0; + _pub_gpos.get().terrain_alt_valid = false; + _pub_gpos.get().dead_reckoning = !_canEstimateXY && !_xyTimeout; + _pub_gpos.update(); + } +} + +void BlockLocalPositionEstimator::publishFilteredFlow() +{ + // publish filtered flow + if (isfinite(_pub_filtered_flow.get().sumx) && + isfinite(_pub_filtered_flow.get().sumy) && + isfinite(_pub_filtered_flow.get().vx) && + isfinite(_pub_filtered_flow.get().vy)) { + _pub_filtered_flow.update(); + } +} + +void BlockLocalPositionEstimator::initP() +{ + _P.setZero(); + _P(X_x, X_x) = 1; + _P(X_y, X_y) = 1; + _P(X_z, X_z) = 1; + _P(X_vx, X_vx) = 1; + _P(X_vy, X_vy) = 1; + _P(X_vz, X_vz) = 1; + _P(X_bx, X_bx) = 1e-6; + _P(X_by, X_by) = 1e-6; + _P(X_bz, X_bz) = 1e-6; +} + +void BlockLocalPositionEstimator::predict() +{ + // if can't update anything, don't propagate + // state or covariance + if (!_canEstimateXY && !_canEstimateZ) { return; } + + if (_integrate.get() && _sub_att.get().R_valid) { + Matrix3f R_att(_sub_att.get().R); + Vector3f a(_sub_sensor.get().accelerometer_m_s2); + Vector3f b(_x(X_bx), _x(X_by), _x(X_bz)); + _u = R_att * (a - b); + _u(U_az) += 9.81f; // add g + + } else { + _u = Vector3f(0, 0, 0); + } + + // dynamics matrix + Matrix A; // state dynamics matrix + A.setZero(); + // derivative of position is velocity + A(X_x, X_vx) = 1; + A(X_y, X_vy) = 1; + A(X_z, X_vz) = 1; + + // derivative of velocity is accelerometer acceleration + // (in input matrix) - bias (in body frame) + Matrix3f R_att(_sub_att.get().R); + A(X_vx, X_bx) = -R_att(0, 0); + A(X_vx, X_by) = -R_att(0, 1); + A(X_vx, X_bz) = -R_att(0, 2); + + A(X_vy, X_bx) = -R_att(1, 0); + A(X_vy, X_by) = -R_att(1, 1); + A(X_vy, X_bz) = -R_att(1, 2); + + A(X_vz, X_bx) = -R_att(2, 0); + A(X_vz, X_by) = -R_att(2, 1); + A(X_vz, X_bz) = -R_att(2, 2); + + // input matrix + Matrix B; // input matrix + B.setZero(); + B(X_vx, U_ax) = 1; + B(X_vy, U_ay) = 1; + B(X_vz, U_az) = 1; + + // input noise covariance matrix + Matrix R; + R.setZero(); + R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get(); + R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get(); + R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get(); + + // process noise power matrix + Matrix Q; + Q.setZero(); + Q(X_x, X_x) = _pn_p_noise_power.get(); + Q(X_y, X_y) = _pn_p_noise_power.get(); + Q(X_z, X_z) = _pn_p_noise_power.get(); + Q(X_vx, X_vx) = _pn_v_noise_power.get(); + Q(X_vy, X_vy) = _pn_v_noise_power.get(); + Q(X_vz, X_vz) = _pn_v_noise_power.get(); + + // technically, the noise is in the body frame, + // but the components are all the same, so + // ignoring for now + Q(X_bx, X_bx) = _pn_b_noise_power.get(); + Q(X_by, X_by) = _pn_b_noise_power.get(); + Q(X_bz, X_bz) = _pn_b_noise_power.get(); + + // continuous time kalman filter prediction + Matrix dx = (A * _x + B * _u) * getDt(); + + // only predict for components we have + // valid measurements for + if (!_canEstimateXY) { + dx(X_x) = 0; + dx(X_y) = 0; + dx(X_vx) = 0; + dx(X_vy) = 0; + } + + if (!_canEstimateZ) { + dx(X_z) = 0; + dx(X_vz) = 0; + } + + // propagate + _x += dx; + _P += (A * _P + _P * A.transpose() + + B * R * B.transpose() + Q) * getDt(); +} + +void BlockLocalPositionEstimator::correctFlow() +{ + + // flow measurement matrix and noise matrix + Matrix C; + C.setZero(); + C(Y_flow_x, X_x) = 1; + C(Y_flow_y, X_y) = 1; + + Matrix R; + R.setZero(); + R(Y_flow_x, Y_flow_x) = + _flow_xy_stddev.get() * _flow_xy_stddev.get(); + R(Y_flow_y, Y_flow_y) = + _flow_xy_stddev.get() * _flow_xy_stddev.get(); + + float flow_speed[3] = {0.0f, 0.0f, 0.0f}; + float global_speed[3] = {0.0f, 0.0f, 0.0f}; + + /* calc dt between flow timestamps */ + /* ignore first flow msg */ + if (_time_last_flow == 0) { + _time_last_flow = _sub_flow.get().timestamp; + return; + } + + float dt = (_sub_flow.get().timestamp - _time_last_flow) * 1.0e-6f ; + _time_last_flow = _sub_flow.get().timestamp; + + // calculate velocity over ground + if (_sub_flow.get().integration_timespan > 0) { + flow_speed[0] = (_sub_flow.get().pixel_flow_x_integral / + (_sub_flow.get().integration_timespan / 1e6f) - + _sub_att.get().pitchspeed) * // Body rotation correction TODO check this + _x(X_z); + flow_speed[1] = (_sub_flow.get().pixel_flow_y_integral / + (_sub_flow.get().integration_timespan / 1e6f) - + _sub_att.get().rollspeed) * // Body rotation correction + _x(X_z); + + } else { + flow_speed[0] = 0; + flow_speed[1] = 0; + } + + flow_speed[2] = 0.0f; + + /* update filtered flow */ + _pub_filtered_flow.get().sumx += flow_speed[0] * dt; + _pub_filtered_flow.get().sumy += flow_speed[1] * dt; + _pub_filtered_flow.get().vx = flow_speed[0]; + _pub_filtered_flow.get().vy = flow_speed[1]; + + // TODO add yaw rotation correction (with distance to vehicle zero) + + // convert to globalframe velocity + for (uint8_t i = 0; i < 3; i++) { + float sum = 0.0f; + + for (uint8_t j = 0; j < 3; j++) { + sum += flow_speed[j] * PX4_R(_sub_att.get().R, i, j); + } + + global_speed[i] = sum; + } + + // flow integral + _flowX += global_speed[0] * dt; + _flowY += global_speed[1] * dt; + + // measurement + Vector2f y; + y(0) = _flowX; + y(1) = _flowY; + + // residual + Vector2f r = y - C * _x; + + // residual covariance, (inverse) + Matrix S_I = + (C * _P * C.transpose() + R).inverse(); + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (_sub_flow.get().quality < MIN_FLOW_QUALITY) { + if (!_flowFault) { + mavlink_log_info(_mavlink_fd, "[lpe] bad flow data "); + warnx("[lpe] bad flow data "); + _flowFault = FAULT_SEVERE; + } + + } else if (beta > _beta_max.get()) { + if (!_flowFault) { + mavlink_log_info(_mavlink_fd, "[lpe] flow fault, beta %5.2f", double(beta)); + warnx("[lpe] flow fault, beta %5.2f", double(beta)); + _flowFault = FAULT_MINOR; + } + + } else if (_flowFault) { + _flowFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] flow OK"); + warnx("[lpe] flow OK"); + } + + // kalman filter correction if no fault + if (_flowFault == FAULT_NONE) { + Matrix K = + _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + // reset flow integral to current estimate of position + // if a fault occurred + + } else { + _flowX = _x(X_x); + _flowY = _x(X_y); + } + +} + +void BlockLocalPositionEstimator::correctSonar() +{ + + if (_sub_distance.get().type != distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND) { + return; + } + + float d = _sub_distance.get().current_distance; + + // sonar measurement matrix and noise matrix + Matrix C; + C.setZero(); + C(Y_sonar_z, X_z) = -1; + + // use parameter covariance unless sensor provides reasonable value + Matrix R; + R.setZero(); + float cov = _sub_distance.get().covariance; + + if (cov < 1.0e-3f) { + R(0, 0) = _sonar_z_stddev.get() * _sonar_z_stddev.get(); + + } else { + R(0, 0) = cov; + } + + // measurement + Matrix y; + y(0) = (d - _sonarAltHome) * + cosf(_sub_att.get().roll) * + cosf(_sub_att.get().pitch); + + // residual + Matrix r = y - C * _x; + + // residual covariance, (inverse) + Matrix S_I = + (C * _P * C.transpose() + R).inverse(); + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (d < _sub_distance.get().min_distance || + d > _sub_distance.get().max_distance) { + if (!_sonarFault) { + mavlink_log_info(_mavlink_fd, "[lpe] sonar out of range"); + warnx("[lpe] sonar out of range"); + _sonarFault = FAULT_SEVERE; + } + + } else if (beta > _beta_max.get()) { + if (!_sonarFault) { + mavlink_log_info(_mavlink_fd, "[lpe] sonar fault, beta %5.2f", double(beta)); + warnx("[lpe] sonar fault, beta %5.2f", double(beta)); + _sonarFault = FAULT_MINOR; + } + + } else if (_sonarFault) { + _sonarFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] sonar OK"); + warnx("[lpe] sonar OK"); + } + + // kalman filter correction if no fault + if (_sonarFault == FAULT_NONE) { + Matrix K = + _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_sonar = _sub_distance.get().timestamp; + +} + +void BlockLocalPositionEstimator::correctBaro() +{ + + Matrix y; + y(0) = _sub_sensor.get().baro_alt_meter[0] - _baroAltHome; + + // baro measurement matrix + Matrix C; + C.setZero(); + C(Y_baro_z, X_z) = -1; // measured altitude, negative down dir. + + Matrix R; + R.setZero(); + R(0, 0) = _baro_stddev.get() * _baro_stddev.get(); + + // residual + Matrix S_I = + ((C * _P * C.transpose()) + R).inverse(); + Matrix r = y - (C * _x); + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (beta > _beta_max.get()) { + if (!_baroFault) { + mavlink_log_info(_mavlink_fd, "[lpe] baro fault, beta %5.2f", double(beta)); + warnx("[lpe] baro fault, beta %5.2f", double(beta)); + _baroFault = FAULT_MINOR; + } + + // lower baro trust + S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + + } else if (_baroFault) { + _baroFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] baro OK"); + warnx("[lpe] baro OK"); + } + + // kalman filter correction if no fault + if (_baroFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_baro = _sub_sensor.get().baro_timestamp[0]; +} + +void BlockLocalPositionEstimator::correctLidar() +{ + + if (_sub_distance.get().type != distance_sensor_s::MAV_DISTANCE_SENSOR_LASER) { + return; + } + + float d = _sub_distance.get().current_distance; + + Matrix C; + C.setZero(); + C(Y_lidar_z, X_z) = -1; // measured altitude, + // negative down dir. + + // use parameter covariance unless sensor provides reasonable value + Matrix R; + R.setZero(); + float cov = _sub_distance.get().covariance; + + if (cov < 1.0e-3f) { + R(0, 0) = _lidar_z_stddev.get() * _lidar_z_stddev.get(); + + } else { + R(0, 0) = cov; + } + + Matrix y; + y.setZero(); + y(0) = (d - _lidarAltHome) * + cosf(_sub_att.get().roll) * + cosf(_sub_att.get().pitch); + + // residual + Matrix S_I = ((C * _P * C.transpose()) + R).inverse(); + Matrix r = y - C * _x; + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + // zero is an error code for the lidar + if (d < _sub_distance.get().min_distance || + d > _sub_distance.get().max_distance) { + if (!_lidarFault) { + mavlink_log_info(_mavlink_fd, "[lpe] lidar out of range"); + warnx("[lpe] lidar out of range"); + _lidarFault = FAULT_SEVERE; + } + + } else if (beta > _beta_max.get()) { + if (!_lidarFault) { + mavlink_log_info(_mavlink_fd, "[lpe] lidar fault, beta %5.2f", double(beta)); + warnx("[lpe] lidar fault, beta %5.2f", double(beta)); + _lidarFault = FAULT_MINOR; + } + + } else if (_lidarFault) { // disable fault if ok + _lidarFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] lidar OK"); + warnx("[lpe] lidar OK"); + } + + // kalman filter correction if no fault + if (_lidarFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_lidar = _sub_distance.get().timestamp; +} + +void BlockLocalPositionEstimator::correctGps() // TODO : use another other metric for glitch detection +{ + + // gps measurement in local frame + double lat = _sub_gps.get().lat * 1.0e-7; + double lon = _sub_gps.get().lon * 1.0e-7; + float alt = _sub_gps.get().alt * 1.0e-3f; + + float px = 0; + float py = 0; + float pz = alt - _gpsAltHome; + map_projection_project(&_map_ref, lat, lon, &px, &py); + + //printf("gps: lat %10g, lon, %10g alt %10g\n", lat, lon, double(alt)); + //printf("home: lat %10g, lon, %10g alt %10g\n", _sub_home.lat, _sub_home.lon, double(_sub_home.alt)); + //printf("local: x %10g y %10g z %10g\n", double(px), double(py), double(pz)); + + Matrix y; + y.setZero(); + y(0) = px; + y(1) = py; + y(2) = pz; + y(3) = _sub_gps.get().vel_n_m_s; + y(4) = _sub_gps.get().vel_e_m_s; + y(5) = _sub_gps.get().vel_d_m_s; + + // gps measurement matrix, measures position and velocity + Matrix C; + C.setZero(); + C(Y_gps_x, X_x) = 1; + C(Y_gps_y, X_y) = 1; + C(Y_gps_z, X_z) = 1; + C(Y_gps_vx, X_vx) = 1; + C(Y_gps_vy, X_vy) = 1; + C(Y_gps_vz, X_vz) = 1; + + // gps covariance matrix + Matrix R; + R.setZero(); + + // default to parameter, use gps cov if provided + float var_xy = _gps_xy_stddev.get() * _gps_xy_stddev.get(); + float var_z = _gps_z_stddev.get() * _gps_z_stddev.get(); + float var_vxy = _gps_vxy_stddev.get() * _gps_vxy_stddev.get(); + float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get(); + + // if field is not zero, set it to the value provided + if (_sub_gps.get().eph > 1e-3f) { + var_xy = _sub_gps.get().eph * _sub_gps.get().eph; + } + + if (_sub_gps.get().epv > 1e-3f) { + var_z = _sub_gps.get().epv * _sub_gps.get().epv; + } + + // TODO is velocity covariance provided from gps sub + R(0, 0) = var_xy; + R(1, 1) = var_xy; + R(2, 2) = var_z; + R(3, 3) = var_vxy; + R(4, 4) = var_vxy; + R(5, 5) = var_vz; + + // residual + Matrix r = y - C * _x; + Matrix S_I = (C * _P * C.transpose() + R).inverse(); + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + uint8_t nSat = _sub_gps.get().satellites_used; + float eph = _sub_gps.get().eph; + + if (nSat < 6 || eph > _gps_eph_max.get()) { + if (!_gpsFault) { + mavlink_log_info(_mavlink_fd, "[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph)); + warnx("[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph)); + _gpsFault = FAULT_SEVERE; + } + + } else if (beta > _beta_max.get()) { + if (!_gpsFault) { + mavlink_log_info(_mavlink_fd, "[lpe] gps fault, beta: %5.2f", double(beta)); + warnx("[lpe] gps fault, beta: %5.2f", double(beta)); + mavlink_log_info(_mavlink_fd, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", + double(r(0)), double(r(1)), double(r(2)), + double(r(3)), double(r(4)), double(r(5))); + mavlink_log_info(_mavlink_fd, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", + double(S_I(0, 0)), double(S_I(1, 1)), double(S_I(2, 2)), + double(S_I(3, 3)), double(S_I(4, 4)), double(S_I(5, 5))); + mavlink_log_info(_mavlink_fd, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", + double(r(0)), double(r(1)), double(r(2)), + double(r(3)), double(r(4)), double(r(5))); + _gpsFault = FAULT_MINOR; + } + + // trust GPS less + S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + + } else if (_gpsFault) { + _gpsFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] GPS OK"); + warnx("[lpe] GPS OK"); + } + + // kalman filter correction if no hard fault + if (_gpsFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_gps = _timeStamp; +} + +void BlockLocalPositionEstimator::correctVision() +{ + + Matrix y; + y.setZero(); + y(0) = _sub_vision_pos.get().x - _visionHome(0); + y(1) = _sub_vision_pos.get().y - _visionHome(1); + y(2) = _sub_vision_pos.get().z - _visionHome(2); + + // vision measurement matrix, measures position + Matrix C; + C.setZero(); + C(Y_vision_x, X_x) = 1; + C(Y_vision_y, X_y) = 1; + C(Y_vision_z, X_z) = 1; + + // noise matrix + Matrix R; + R.setZero(); + R(Y_vision_x, Y_vision_x) = _vision_xy_stddev.get() * _vision_xy_stddev.get(); + R(Y_vision_y, Y_vision_y) = _vision_xy_stddev.get() * _vision_xy_stddev.get(); + R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get(); + + // residual + Matrix S_I = ((C * _P * C.transpose()) + R).inverse(); + Matrix r = y - C * _x; + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (beta > _beta_max.get()) { + if (!_visionFault) { + mavlink_log_info(_mavlink_fd, "[lpe] vision position fault, beta %5.2f", double(beta)); + warnx("[lpe] vision position fault, beta %5.2f", double(beta)); + _visionFault = FAULT_MINOR; + } + + // trust less + S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + + } else if (_visionFault) { + _visionFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] vision position OK"); + warnx("[lpe] vision position OK"); + } + + // kalman filter correction if no fault + if (_visionFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_vision_p = _sub_vision_pos.get().timestamp_boot; +} + +void BlockLocalPositionEstimator::correctmocap() +{ + + Matrix y; + y.setZero(); + y(Y_mocap_x) = _sub_mocap.get().x - _mocapHome(0); + y(Y_mocap_y) = _sub_mocap.get().y - _mocapHome(1); + y(Y_mocap_z) = _sub_mocap.get().z - _mocapHome(2); + + // mocap measurement matrix, measures position + Matrix C; + C.setZero(); + C(Y_mocap_x, X_x) = 1; + C(Y_mocap_y, X_y) = 1; + C(Y_mocap_z, X_z) = 1; + + // noise matrix + Matrix R; + R.setZero(); + float mocap_p_var = _mocap_p_stddev.get()* \ + _mocap_p_stddev.get(); + R(Y_mocap_x, Y_mocap_x) = mocap_p_var; + R(Y_mocap_y, Y_mocap_y) = mocap_p_var; + R(Y_mocap_z, Y_mocap_z) = mocap_p_var; + + // residual + Matrix S_I = ((C * _P * C.transpose()) + R).inverse(); + Matrix r = y - C * _x; + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (beta > _beta_max.get()) { + if (!_mocapFault) { + mavlink_log_info(_mavlink_fd, "[lpe] mocap fault, beta %5.2f", double(beta)); + warnx("[lpe] mocap fault, beta %5.2f", double(beta)); + _mocapFault = FAULT_MINOR; + } + + // trust less + S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + + } else if (_mocapFault) { + _mocapFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] mocap OK"); + warnx("[lpe] mocap OK"); + } + + // kalman filter correction if no fault + if (_mocapFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_mocap = _sub_mocap.get().timestamp_boot; +} diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp new file mode 100644 index 0000000000..66f301efe3 --- /dev/null +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -0,0 +1,310 @@ +#pragma once + +#include +#include +#include +#include + +#ifdef USE_MATRIX_LIB +#include "matrix/src/Matrix.hpp" +using namespace matrix; +#else +#include +using namespace Eigen; +#endif + +// uORB Subscriptions +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// uORB Publications +#include +#include +#include +#include +#include + +#define CBRK_NO_VISION_KEY 328754 + +using namespace control; + +enum fault_t { + FAULT_NONE = 0, + FAULT_MINOR, + FAULT_SEVERE +}; + +enum sensor_t { + SENSOR_BARO = 0, + SENSOR_GPS, + SENSOR_LIDAR, + SENSOR_FLOW, + SENSOR_SONAR, + SENSOR_VISION, + SENSOR_MOCAP +}; + +class BlockLocalPositionEstimator : public control::SuperBlock +{ +// +// The purpose of this estimator is to provide a robust solution for +// indoor flight. +// +// dynamics: +// +// x(+) = A * x(-) + B * u(+) +// y_i = C_i*x +// +// kalman filter +// +// E[xx'] = P +// E[uu'] = W +// E[y_iy_i'] = R_i +// +// prediction +// x(+|-) = A*x(-|-) + B*u(+) +// P(+|-) = A*P(-|-)*A' + B*W*B' +// +// correction +// x(+|+) = x(+|-) + K_i * (y_i - H_i * x(+|-) ) +// +// +// input: +// ax, ay, az (acceleration NED) +// +// states: +// px, py, pz , ( position NED) +// vx, vy, vz ( vel NED), +// bx, by, bz ( TODO accelerometer bias) +// tz (TODO terrain altitude) +// +// measurements: +// +// sonar: pz (measured d*cos(phi)*cos(theta)) +// +// baro: pz +// +// flow: vx, vy (flow is in body x, y frame) +// +// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame) +// +// lidar: px (actual measured d*cos(phi)*cos(theta)) +// +// vision: px, py, pz, vx, vy, vz +// +// mocap: px, py, pz +// +public: + BlockLocalPositionEstimator(); + void update(); + virtual ~BlockLocalPositionEstimator(); + +private: + // prevent copy and assignment + BlockLocalPositionEstimator(const BlockLocalPositionEstimator &); + BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &); + + // constants + static const uint8_t n_x = 9; + static const uint8_t n_u = 3; // 3 accelerations + static const uint8_t n_y_flow = 2; + static const uint8_t n_y_sonar = 1; + static const uint8_t n_y_baro = 1; + static const uint8_t n_y_lidar = 1; + static const uint8_t n_y_gps = 6; + static const uint8_t n_y_vision = 3; + static const uint8_t n_y_mocap = 3; + enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz}; + enum {U_ax = 0, U_ay, U_az}; + enum {Y_baro_z = 0}; + enum {Y_lidar_z = 0}; + enum {Y_flow_x = 0, Y_flow_y}; + enum {Y_sonar_z = 0}; + enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz}; + enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, Y_vision_vx, Y_vision_vy, Y_vision_vz}; + enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z}; + enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM}; + + // methods + // ---------------------------- + void initP(); + + // predict the next state + void predict(); + + // correct the state prediction wtih a measurement + void correctBaro(); + void correctGps(); + void correctLidar(); + void correctFlow(); + void correctSonar(); + void correctVision(); + void correctmocap(); + + // sensor initialization + void updateHome(); + void initBaro(); + void initGps(); + void initLidar(); + void initSonar(); + void initFlow(); + void initVision(); + void initmocap(); + + // publications + void publishLocalPos(); + void publishGlobalPos(); + void publishFilteredFlow(); + void publishEstimatorStatus(); + + // attributes + // ---------------------------- + + // subscriptions + uORB::Subscription _sub_status; + uORB::Subscription _sub_armed; + uORB::Subscription _sub_control_mode; + uORB::Subscription _sub_att; + uORB::Subscription _sub_att_sp; + uORB::Subscription _sub_flow; + uORB::Subscription _sub_sensor; + uORB::Subscription _sub_distance; + uORB::Subscription _sub_param_update; + uORB::Subscription _sub_manual; + uORB::Subscription _sub_home; + uORB::Subscription _sub_gps; + uORB::Subscription _sub_vision_pos; + uORB::Subscription _sub_mocap; + + // publications + uORB::Publication _pub_lpos; + uORB::Publication _pub_gpos; + uORB::Publication _pub_filtered_flow; + uORB::Publication _pub_est_status; + + // map projection + struct map_projection_reference_s _map_ref; + + // parameters + BlockParamInt _integrate; + + BlockParamFloat _flow_xy_stddev; + BlockParamFloat _sonar_z_stddev; + + BlockParamFloat _lidar_z_stddev; + + BlockParamFloat _accel_xy_stddev; + BlockParamFloat _accel_z_stddev; + + BlockParamFloat _baro_stddev; + + BlockParamFloat _gps_xy_stddev; + BlockParamFloat _gps_z_stddev; + + BlockParamFloat _gps_vxy_stddev; + BlockParamFloat _gps_vz_stddev; + + BlockParamFloat _gps_eph_max; + + BlockParamFloat _vision_xy_stddev; + BlockParamFloat _vision_z_stddev; + BlockParamInt _no_vision; + BlockParamFloat _beta_max; + + BlockParamFloat _mocap_p_stddev; + + // process noise + BlockParamFloat _pn_p_noise_power; + BlockParamFloat _pn_v_noise_power; + BlockParamFloat _pn_b_noise_power; + + // misc + struct pollfd _polls[3]; + uint64_t _timeStamp; + uint64_t _time_last_xy; + uint64_t _time_last_flow; + uint64_t _time_last_baro; + uint64_t _time_last_gps; + uint64_t _time_last_lidar; + uint64_t _time_last_sonar; + uint64_t _time_last_vision_p; + uint64_t _time_last_mocap; + int _mavlink_fd; + + // initialization flags + bool _baroInitialized; + bool _gpsInitialized; + bool _lidarInitialized; + bool _sonarInitialized; + bool _flowInitialized; + bool _visionInitialized; + bool _mocapInitialized; + + // init counts + int _baroInitCount; + int _gpsInitCount; + int _lidarInitCount; + int _sonarInitCount; + int _flowInitCount; + int _visionInitCount; + int _mocapInitCount; + + // reference altitudes + float _altHome; + bool _altHomeInitialized; + float _baroAltHome; + float _gpsAltHome; + float _lidarAltHome; + float _sonarAltHome; + float _flowAltHome; + Vector3f _visionHome; + Vector3f _mocapHome; + + // flow integration + float _flowX; + float _flowY; + float _flowMeanQual; + + // referene lat/lon + double _gpsLatHome; + double _gpsLonHome; + + // status + bool _canEstimateXY; + bool _canEstimateZ; + bool _xyTimeout; + + // sensor faults + fault_t _baroFault; + fault_t _gpsFault; + fault_t _lidarFault; + fault_t _flowFault; + fault_t _sonarFault; + fault_t _visionFault; + fault_t _mocapFault; + + bool _visionTimeout; + bool _mocapTimeout; + + perf_counter_t _loop_perf; + perf_counter_t _interval_perf; + perf_counter_t _err_perf; + + // state space + Matrix _x; // state vector + Matrix _u; // input vector + Matrix _P; // state covariance matrix +}; diff --git a/src/modules/local_position_estimator/CMakeLists.txt b/src/modules/local_position_estimator/CMakeLists.txt new file mode 100644 index 0000000000..e0d1be2d0a --- /dev/null +++ b/src/modules/local_position_estimator/CMakeLists.txt @@ -0,0 +1,57 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +if(${OS} STREQUAL "nuttx") + list(APPEND MODULE_CFLAGS -Wframe-larger-than=6500) +elseif(${OS} STREQUAL "posix") + list(APPEND MODULE_CFLAGS -Wno-error) + # add matrix tests + add_subdirectory(matrix) +endif() + +# use custom matrix lib instead of Eigen +add_definitions(-DUSE_MATRIX_LIB) + + +px4_add_module( + MODULE modules__local_position_estimator + MAIN local_position_estimator + STACK 9216 + COMPILE_FLAGS ${MODULE_CFLAGS} + SRCS + local_position_estimator_main.cpp + BlockLocalPositionEstimator.cpp + params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/local_position_estimator/local_position_estimator_main.cpp b/src/modules/local_position_estimator/local_position_estimator_main.cpp new file mode 100644 index 0000000000..b0cd25554e --- /dev/null +++ b/src/modules/local_position_estimator/local_position_estimator_main.cpp @@ -0,0 +1,169 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file local_position_estimator.cpp + * @author James Goppert + * @author Mohammed Kabir + * @author Nuno Marques + * + * Local position estimator + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "BlockLocalPositionEstimator.hpp" + +static volatile bool thread_should_exit = false; /**< Deamon exit flag */ +static volatile bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int local_position_estimator_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int local_position_estimator_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static int usage(const char *reason); + +static int +usage(const char *reason) +{ + if (reason) { + fprintf(stderr, "%s\n", reason); + } + + fprintf(stderr, "usage: local_position_estimator {start|stop|status} [-p ]\n\n"); + return 1; +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int local_position_estimator_main(int argc, char *argv[]) +{ + + if (argc < 1) { + usage("missing command"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + return 0; + } + + thread_should_exit = false; + + deamon_task = px4_task_spawn_cmd("lp_estimator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 10240, + local_position_estimator_thread_main, + (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL); + return 0; + } + + if (!strcmp(argv[1], "stop")) { + if (thread_running) { + warnx("stop"); + thread_should_exit = true; + + } else { + warnx("not started"); + } + + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + return 0; + } + + usage("unrecognized command"); + return 1; +} + +int local_position_estimator_thread_main(int argc, char *argv[]) +{ + + warnx("starting"); + + using namespace control; + + + thread_running = true; + + BlockLocalPositionEstimator est; + + while (!thread_should_exit) { + est.update(); + } + + warnx("exiting."); + + thread_running = false; + + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/.gitignore b/src/modules/local_position_estimator/matrix/.gitignore new file mode 100644 index 0000000000..a5309e6b90 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/.gitignore @@ -0,0 +1 @@ +build*/ diff --git a/src/modules/local_position_estimator/matrix/CMakeLists.txt b/src/modules/local_position_estimator/matrix/CMakeLists.txt new file mode 100644 index 0000000000..5a16ced707 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 2.8) + +project(matrix CXX) + +set(CMAKE_BUILD_TYPE "RelWithDebInfo") + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Weffc++") + +enable_testing() + +include_directories(src) + +add_subdirectory(test) diff --git a/src/modules/local_position_estimator/matrix/src/Matrix.hpp b/src/modules/local_position_estimator/matrix/src/Matrix.hpp new file mode 100644 index 0000000000..8a0432b7ef --- /dev/null +++ b/src/modules/local_position_estimator/matrix/src/Matrix.hpp @@ -0,0 +1,459 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace matrix +{ + +template +class Matrix +{ + +private: + T _data[M][N]; + size_t _rows; + size_t _cols; + +public: + + Matrix() : + _data(), + _rows(M), + _cols(N) + { + } + + Matrix(const T *data) : + _data(), + _rows(M), + _cols(N) + { + memcpy(_data, data, sizeof(_data)); + } + + Matrix(const Matrix &other) : + _data(), + _rows(M), + _cols(N) + { + memcpy(_data, other._data, sizeof(_data)); + } + + Matrix(T x, T y, T z) : + _data(), + _rows(M), + _cols(N) + { + // TODO, limit to only 3x1 matrices + _data[0][0] = x; + _data[1][0] = y; + _data[2][0] = z; + } + + /** + * Accessors/ Assignment etc. + */ + + inline size_t rows() const + { + return _rows; + } + + inline size_t cols() const + { + return _rows; + } + + inline T operator()(size_t i) const + { + return _data[i][0]; + } + + inline T operator()(size_t i, size_t j) const + { + return _data[i][j]; + } + + inline T &operator()(size_t i) + { + return _data[i][0]; + } + + inline T &operator()(size_t i, size_t j) + { + return _data[i][j]; + } + + /** + * Matrix Operations + */ + + // this might use a lot of programming memory + // since it instantiates a class for every + // required mult pair, but it provides + // compile time size_t checking + template + Matrix operator*(const Matrix &other) const + { + const Matrix &self = *this; + Matrix res; + res.setZero(); + + for (size_t i = 0; i < M; i++) { + for (size_t k = 0; k < P; k++) { + for (size_t j = 0; j < N; j++) { + res(i, k) += self(i, j) * other(j, k); + } + } + } + + return res; + } + + Matrix operator+(const Matrix &other) const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + res(i , j) = self(i, j) + other(i, j); + } + } + + return res; + } + + bool operator==(const Matrix &other) const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + if (self(i , j) != other(i, j)) { + return false; + } + } + } + + return true; + } + + Matrix operator-(const Matrix &other) const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + res(i , j) = self(i, j) - other(i, j); + } + } + + return res; + } + + void operator+=(const Matrix &other) + { + Matrix &self = *this; + self = self + other; + } + + void operator-=(const Matrix &other) + { + Matrix &self = *this; + self = self - other; + } + + void operator*=(const Matrix &other) + { + Matrix &self = *this; + self = self * other; + } + + /** + * Scalar Operations + */ + + Matrix operator*(T scalar) const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + res(i , j) = self(i, j) * scalar; + } + } + + return res; + } + + Matrix operator+(T scalar) const + { + Matrix res; + Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + res(i , j) = self(i, j) + scalar; + } + } + + return res; + } + + void operator*=(T scalar) + { + Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + self(i, j) = self(i, j) * scalar; + } + } + } + + void operator/=(T scalar) + { + Matrix &self = *this; + self = self * (1.0f / scalar); + } + + /** + * Misc. Functions + */ + + void print() + { + Matrix &self = *this; + printf("\n"); + + for (size_t i = 0; i < M; i++) { + printf("["); + + for (size_t j = 0; j < N; j++) { + printf("%10g\t", double(self(i, j))); + } + + printf("]\n"); + } + } + + Matrix transpose() const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + res(j, i) = self(i, j); + } + } + + return res; + } + + Matrix expm(float dt, size_t n) const + { + Matrix res; + res.setIdentity(); + Matrix A_pow = *this; + float k_fact = 1; + size_t k = 1; + + while (k < n) { + res += A_pow * (float(pow(dt, k)) / k_fact); + + if (k == n) { break; } + + A_pow *= A_pow; + k_fact *= k; + k++; + } + + return res; + } + + Matrix diagonal() const + { + Matrix res; + // force square for now + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + res(i) = self(i, i); + } + + return res; + } + + void setZero() + { + memset(_data, 0, sizeof(_data)); + } + + void setIdentity() + { + setZero(); + Matrix &self = *this; + + for (size_t i = 0; i < M and i < N; i++) { + self(i, i) = 1; + } + } + + inline void swapRows(size_t a, size_t b) + { + if (a == b) { return; } + + Matrix &self = *this; + + for (size_t j = 0; j < cols(); j++) { + T tmp = self(a, j); + self(a, j) = self(b, j); + self(b, j) = tmp; + } + } + + inline void swapCols(size_t a, size_t b) + { + if (a == b) { return; } + + Matrix &self = *this; + + for (size_t i = 0; i < rows(); i++) { + T tmp = self(i, a); + self(i, a) = self(i, b); + self(i, b) = tmp; + } + } + + /** + * inverse based on LU factorization with partial pivotting + */ + Matrix inverse() const + { + Matrix L; + L.setIdentity(); + const Matrix &A = (*this); + Matrix U = A; + Matrix P; + P.setIdentity(); + + //printf("A:\n"); A.print(); + + // for all diagonal elements + for (size_t n = 0; n < N; n++) { + + // if diagonal is zero, swap with row below + if (fabsf(U(n, n)) < 1e-8f) { + //printf("trying pivot for row %d\n",n); + for (size_t i = 0; i < N; i++) { + if (i == n) { continue; } + + //printf("\ttrying row %d\n",i); + if (fabsf(U(i, n)) > 1e-8f) { + //printf("swapped %d\n",i); + U.swapRows(i, n); + P.swapRows(i, n); + } + } + } + +#ifdef MATRIX_ASSERT + //printf("A:\n"); A.print(); + //printf("U:\n"); U.print(); + //printf("P:\n"); P.print(); + //fflush(stdout); + ASSERT(fabsf(U(n, n)) > 1e-8f); +#endif + + // failsafe, return zero matrix + if (fabsf(U(n, n)) < 1e-8f) { + Matrix zero; + zero.setZero(); + return zero; + } + + // for all rows below diagonal + for (size_t i = (n + 1); i < N; i++) { + L(i, n) = U(i, n) / U(n, n); + + // add i-th row and n-th row + // multiplied by: -a(i,n)/a(n,n) + for (size_t k = n; k < N; k++) { + U(i, k) -= L(i, n) * U(n, k); + } + } + } + + //printf("L:\n"); L.print(); + //printf("U:\n"); U.print(); + + // solve LY=P*I for Y by forward subst + Matrix Y = P; + + // for all columns of Y + for (size_t c = 0; c < N; c++) { + // for all rows of L + for (size_t i = 0; i < N; i++) { + // for all columns of L + for (size_t j = 0; j < i; j++) { + // for all existing y + // subtract the component they + // contribute to the solution + Y(i, c) -= L(i, j) * Y(j, c); + } + + // divide by the factor + // on current + // term to be solved + // Y(i,c) /= L(i,i); + // but L(i,i) = 1.0 + } + } + + //printf("Y:\n"); Y.print(); + + // solve Ux=y for x by back subst + Matrix X = Y; + + // for all columns of X + for (size_t c = 0; c < N; c++) { + // for all rows of U + for (size_t k = 0; k < N; k++) { + // have to go in reverse order + size_t i = N - 1 - k; + + // for all columns of U + for (size_t j = i + 1; j < N; j++) { + // for all existing x + // subtract the component they + // contribute to the solution + X(i, c) -= U(i, j) * X(j, c); + } + + // divide by the factor + // on current + // term to be solved + X(i, c) /= U(i, i); + } + } + + //printf("X:\n"); X.print(); + return X; + } + +}; + +typedef Matrix Vector2f; +typedef Matrix Vector3f; +typedef Matrix Matrix3f; + +}; // namespace matrix diff --git a/src/modules/local_position_estimator/matrix/test/CMakeLists.txt b/src/modules/local_position_estimator/matrix/test/CMakeLists.txt new file mode 100644 index 0000000000..cf280e287b --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/CMakeLists.txt @@ -0,0 +1,15 @@ +set(tests + setIdentity + inverse + matrixMult + vectorAssignment + matrixAssignment + matrixScalarMult + transpose + ) + +foreach(test ${tests}) + add_executable(${test} + ${test}.cpp) + add_test(${test} ${test}) +endforeach() diff --git a/src/modules/local_position_estimator/matrix/test/inverse.cpp b/src/modules/local_position_estimator/matrix/test/inverse.cpp new file mode 100644 index 0000000000..73f7c0b432 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/inverse.cpp @@ -0,0 +1,30 @@ +#include "Matrix.hpp" +#include +#include + +using namespace matrix; + +int main() +{ + float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1}; + Matrix3f A(data); + Matrix3f A_I = A.inverse(); + float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1}; + Matrix3f A_I_check(data_check); + (void)A_I; + assert(A_I == A_I_check); + + // stess test + static const size_t n = 100; + Matrix A_large; + A_large.setIdentity(); + Matrix A_large_I; + A_large_I.setZero(); + + for (size_t i = 0; i < 100; i++) { + A_large_I = A_large.inverse(); + assert(A_large == A_large_I); + } + + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp b/src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp new file mode 100644 index 0000000000..5a625d0cc1 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp @@ -0,0 +1,29 @@ +#include "Matrix.hpp" +#include + +using namespace matrix; + +int main() +{ + Matrix3f m; + m.setZero(); + m(0, 0) = 1; + m(0, 1) = 2; + m(0, 2) = 3; + m(1, 0) = 4; + m(1, 1) = 5; + m(1, 2) = 6; + m(2, 0) = 7; + m(2, 1) = 8; + m(2, 2) = 9; + + m.print(); + + float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; + Matrix3f m2(data); + m2.print(); + + assert(m == m2); + + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/matrixMult.cpp b/src/modules/local_position_estimator/matrix/test/matrixMult.cpp new file mode 100644 index 0000000000..7b42d947d4 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/matrixMult.cpp @@ -0,0 +1,26 @@ +#include "Matrix.hpp" +#include +#include + +using namespace matrix; + +int main() +{ + float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1}; + Matrix3f A(data); + float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1}; + Matrix3f A_I(data_check); + Matrix3f I; + I.setIdentity(); + A.print(); + A_I.print(); + Matrix3f R = A * A_I; + R.print(); + assert(R == I); + + Matrix3f R2 = A; + R2 *= A_I; + R2.print(); + assert(R2 == I); + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp b/src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp new file mode 100644 index 0000000000..78bdb5b27f --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp @@ -0,0 +1,18 @@ +#include "Matrix.hpp" +#include +#include + +using namespace matrix; + +int main() +{ + float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; + Matrix3f A(data); + A = A * 2; + float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; + Matrix3f A_check(data_check); + A.print(); + A_check.print(); + assert(A == A_check); + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/setIdentity.cpp b/src/modules/local_position_estimator/matrix/test/setIdentity.cpp new file mode 100644 index 0000000000..f80e437939 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/setIdentity.cpp @@ -0,0 +1,25 @@ +#include "Matrix.hpp" +#include + +using namespace matrix; + +int main() +{ + Matrix3f A; + A.setIdentity(); + assert(A.rows() == 3); + assert(A.cols() == 3); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + if (i == j) { + assert(A(i, j) == 1); + + } else { + assert(A(i, j) == 0); + } + } + } + + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/transpose.cpp b/src/modules/local_position_estimator/matrix/test/transpose.cpp new file mode 100644 index 0000000000..3623fc1f9a --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/transpose.cpp @@ -0,0 +1,18 @@ +#include "Matrix.hpp" +#include +#include + +using namespace matrix; + +int main() +{ + float data[9] = {1, 2, 3, 4, 5, 6}; + Matrix A(data); + Matrix A_T = A.transpose(); + float data_check[9] = {1, 4, 2, 5, 3, 6}; + Matrix A_T_check(data_check); + A_T.print(); + A_T_check.print(); + assert(A_T == A_T_check); + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp b/src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp new file mode 100644 index 0000000000..68f5070194 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp @@ -0,0 +1,28 @@ +#include "Matrix.hpp" +#include + +using namespace matrix; + +int main() +{ + Vector3f v; + v(0) = 1; + v(1) = 2; + v(2) = 3; + + v.print(); + + assert(v(0) == 1); + assert(v(1) == 2); + assert(v(2) == 3); + + Vector3f v2(4, 5, 6); + + v2.print(); + + assert(v2(0) == 4); + assert(v2(1) == 5); + assert(v2(2) == 6); + + return 0; +} diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c new file mode 100644 index 0000000000..8fbde4ab69 --- /dev/null +++ b/src/modules/local_position_estimator/params.c @@ -0,0 +1,222 @@ +#include + +// 16 is max name length + + +/** + * Enable local position estimator. + * + * @group Local Position Estimator + */ +PARAM_DEFINE_INT32(LPE_ENABLED, 1); + +/** + * Enable accelerometer integration for prediction. + * + * @group Local Position Estimator + */ +PARAM_DEFINE_INT32(LPE_INTEGRATE, 1); + +/** + * Optical flow xy standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f); + +/** + * Sonar z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_SNR_Z, 0.2f); + +/** + * Lidar z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f); + +/** + * Accelerometer xy standard deviation + * + * Data sheet sqrt(Noise power) = 150ug/sqrt(Hz) + * std dev = (150*9.8*1e-6)*sqrt(1000 Hz) m/s^2 + * Since accels sampled at 1000 Hz. + * + * should be 0.0464 + * + * @group Local Position Estimator + * @unit m/s^2 + * @min 0.00001 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f); + +/** + * Accelerometer z standard deviation + * + * (see Accel x comments) + * + * @group Local Position Estimator + * @unit m/s^2 + * @min 0.00001 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f); + +/** + * Barometric presssure altitude z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 3 + */ +PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f); + +/** + * GPS xy standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 5 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f); + +/** + * GPS z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 20 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_Z, 10.0f); + +/** + * GPS xy velocity standard deviation. + * + * @group Local Position Estimator + * @unit m/s + * @min 0.01 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.275f); + +/** + * GPS z velocity standard deviation. + * + * @group Local Position Estimator + * @unit m/s + * @min 0.01 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.237f); + +/** + * GPS max eph + * + * @group Local Position Estimator + * @unit m + * @min 1.0 + * @max 5.0 + */ +PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f); + + + +/** + * Vision xy standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f); + +/** + * Vision z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f); + +/** + * Circuit breaker to disable vision input. + * + * Set to the appropriate key (328754) to disable vision input. + * + * @group Local Position Estimator + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(LPE_NO_VISION, 0); + +/** + * Vicon position standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f); + +/** + * Position propagation process noise power (variance*sampling rate). + * + * @group Local Position Estimator + * @unit (m/s^2)-s + * @min 0 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f); + +/** + * Velocity propagation process noise power (variance*sampling rate). + * + * @group Local Position Estimator + * @unit (m/s)-s + * @min 0 + * @max 5 + */ +PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f); + +/** + * Accel bias propagation process noise power (variance*sampling rate). + * + * @group Local Position Estimator + * @unit (m/s)-s + * @min 0 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-8f); + +/** + * Fault detection threshold, for chi-squared dist. + * + * TODO add separate params for 1 dof, 3 dof, and 6 dof beta + * or false alarm rate in false alarms/hr + * + * @group Local Position Estimator + * @unit + * @min 3 + * @max 1000 + */ +PARAM_DEFINE_FLOAT(LPE_BETA_MAX, 1000.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fed42bef2b..be3ab850f4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -1271,6 +1272,78 @@ protected: }; +class MavlinkStreamLocalPositionNEDCOV : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamLocalPositionNEDCOV::get_name_static(); + } + + static const char *get_name_static() + { + return "LOCAL_POSITION_NED_COV"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamLocalPositionNEDCOV(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_est_sub; + uint64_t _est_time; + + /* do not allow top copying this class */ + MavlinkStreamLocalPositionNEDCOV(MavlinkStreamLocalPositionNEDCOV &); + MavlinkStreamLocalPositionNEDCOV& operator = (const MavlinkStreamLocalPositionNEDCOV &); + +protected: + explicit MavlinkStreamLocalPositionNEDCOV(Mavlink *mavlink) : MavlinkStream(mavlink), + _est_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))), + _est_time(0) + {} + + void send(const hrt_abstime t) + { + struct estimator_status_s est; + + if (_est_sub->update(&_est_time, &est)) { + mavlink_local_position_ned_cov_t msg; + + msg.time_boot_ms = est.timestamp / 1000; + msg.x = est.states[0]; + msg.y = est.states[1]; + msg.z = est.states[2]; + msg.vx = est.states[3]; + msg.vy = est.states[4]; + msg.vz = est.states[5]; + msg.ax = est.states[6]; + msg.ay = est.states[7]; + msg.az = est.states[8]; + for (int i=0;i<9;i++) { + msg.covariance[i] = est.covariances[i]; + } + msg.covariance[9] = est.nan_flags; + msg.covariance[10] = est.health_flags; + msg.covariance[11] = est.timeout_flags; + memcpy(msg.covariance, est.covariances, sizeof(est.covariances)); + + _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, &msg); + } + } +}; + class MavlinkStreamAttPosMocap : public MavlinkStream { public: @@ -2478,6 +2551,7 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), + new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static), new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static), new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 60747854cd..80847d8e47 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -155,6 +155,10 @@ private: control::BlockParamFloat _manual_thr_min; control::BlockParamFloat _manual_thr_max; + control::BlockDerivative _vel_x_deriv; + control::BlockDerivative _vel_y_deriv; + control::BlockDerivative _vel_z_deriv; + struct { param_t thr_min; param_t thr_max; @@ -331,6 +335,9 @@ MulticopterPositionControl::MulticopterPositionControl() : _global_vel_sp_pub(nullptr), _manual_thr_min(this, "MANTHR_MIN"), _manual_thr_max(this, "MANTHR_MAX"), + _vel_x_deriv(this, "VELD"), + _vel_y_deriv(this, "VELD"), + _vel_z_deriv(this, "VELD"), _ref_alt(0.0f), _ref_timestamp(0), @@ -1039,6 +1046,9 @@ MulticopterPositionControl::task_main() float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; t_prev = t; + // set dt for control blocks + setDt(dt); + if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ _reset_pos_sp = true; @@ -1217,8 +1227,12 @@ MulticopterPositionControl::task_main() /* velocity error */ math::Vector<3> vel_err = _vel_sp - _vel; - /* derivative of velocity error, not includes setpoint acceleration */ - math::Vector<3> vel_err_d = (_vel - _vel_prev) / dt; + /* derivative of velocity error, / + * does not includes setpoint acceleration */ + math::Vector<3> vel_err_d; + vel_err_d(0) = _vel_x_deriv.update(-_vel(0)); + vel_err_d(1) = _vel_y_deriv.update(-_vel(1)); + vel_err_d(2) = _vel_z_deriv.update(-_vel(2)); /* thrust vector in NED frame */ math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 09843de1f1..0fa51f6f0c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -303,3 +303,11 @@ PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.5f); */ PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.5f); +/** + * Low pass filter cut freq. for numerical velocity derivative + * + * @unit Hz + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f); diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index c925574b42..0544131705 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -366,10 +366,10 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte #if 1 // Use this to debug busy CPU that keeps rescheduling with 0 period time - if (interval < HRT_INTERVAL_MIN) { - PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval); - PX4_BACKTRACE(); - } + /*if (interval < HRT_INTERVAL_MIN) {*/ + /*PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval);*/ + /*PX4_BACKTRACE();*/ + /*}*/ #endif entry->deadline = deadline;