diff --git a/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow b/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow index a7c9b28963..595c06b045 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow +++ b/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow @@ -14,13 +14,6 @@ then param set EKF2_EVP_NOISE 0.05 param set EKF2_EVA_NOISE 0.05 - # INAV - param set INAV_LIDAR_EST 1 - param set INAV_W_XY_FLOW 1 - param set INAV_W_XY_GPS_P 0 - param set INAV_W_XY_GPS_V 0 - param set INAV_W_Z_GPS_P 0 - # LPE: Flow-only mode param set LPE_FUSION 242 param set LPE_FAKE_ORIGIN 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision b/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision index b20e1886be..7bf135d792 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision +++ b/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision @@ -13,13 +13,6 @@ then param set EKF2_AID_MASK 24 param set EKF2_EV_DELAY 5 - # INAV: trust more on the vision input - param set INAV_W_XY_VIS_P 9 - param set INAV_W_Z_VIS_P 7 - param set INAV_W_XY_GPS_P 0 - param set INAV_W_XY_GPS_V 0 - param set INAV_W_Z_GPS_P 0 - # LPE: Vision + baro param set LPE_FUSION 132 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo b/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo index 1590b71abc..87bfcabb5e 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo +++ b/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo @@ -25,11 +25,6 @@ then param set MPC_XY_VEL_MAX 3 param set MPC_Z_VEL_MAX_DN 2 - # INAV: higher GPS weights for better altitude control - param set INAV_W_Z_BARO 0.3 - param set INAV_W_Z_GPS_P 0.8 - param set INAV_W_Z_GPS_V 0.8 - # takeoff, land and RTL settings param set MIS_TAKEOFF_ALT 4 param set RTL_LAND_DELAY 1 diff --git a/boards/aerotenna/ocpoc/ubuntu.cmake b/boards/aerotenna/ocpoc/ubuntu.cmake index ee06da570d..bfe09a3c9a 100644 --- a/boards/aerotenna/ocpoc/ubuntu.cmake +++ b/boards/aerotenna/ocpoc/ubuntu.cmake @@ -76,7 +76,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 2b2a037f4a..c35ff05a27 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -107,7 +107,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index 37aeb92182..0979f8584b 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -117,7 +117,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/auav/x21/default.cmake b/boards/auav/x21/default.cmake index 6ec2ea23d8..30aa0e77a6 100644 --- a/boards/auav/x21/default.cmake +++ b/boards/auav/x21/default.cmake @@ -113,7 +113,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index e99d54ef84..c6b9177d85 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -113,7 +113,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/beaglebone/blue/cross.cmake b/boards/beaglebone/blue/cross.cmake index 7c15ea1435..70ad59e04a 100644 --- a/boards/beaglebone/blue/cross.cmake +++ b/boards/beaglebone/blue/cross.cmake @@ -73,7 +73,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/beaglebone/blue/native.cmake b/boards/beaglebone/blue/native.cmake index 747dc206ac..1374ad6f60 100644 --- a/boards/beaglebone/blue/native.cmake +++ b/boards/beaglebone/blue/native.cmake @@ -71,7 +71,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/emlid/navio2/cross.cmake b/boards/emlid/navio2/cross.cmake index 2a6e9657a5..367fa6acd1 100644 --- a/boards/emlid/navio2/cross.cmake +++ b/boards/emlid/navio2/cross.cmake @@ -81,7 +81,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/emlid/navio2/native.cmake b/boards/emlid/navio2/native.cmake index 20addc3f0f..781ad8f32b 100644 --- a/boards/emlid/navio2/native.cmake +++ b/boards/emlid/navio2/native.cmake @@ -79,7 +79,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index 78fa57dca0..0f24e48135 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -108,7 +108,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index 675f43bf43..a73d2fcc6b 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -119,7 +119,6 @@ px4_add_board( #hello #hwtest # Hardware test #matlab_csv_serial - #position_estimator_inav #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html #rover_steering_control # Rover example app diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index c2d195ca14..9deece41a4 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -121,7 +121,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index e26982fb63..1e4a4a4305 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -121,7 +121,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 848ac86b23..4ca73855bb 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -105,7 +105,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index 6e52ef65fc..a7f747616d 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -108,7 +108,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index c06ffe17cd..7ea6552244 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -105,7 +105,6 @@ px4_add_board( #hello #hwtest # Hardware test #matlab_csv_serial - #position_estimator_inav #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html #rover_steering_control # Rover example app diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 116fccbe3d..b8f2671834 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -119,7 +119,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index 6fa41515fa..65dab30976 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -119,7 +119,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index ab8e819d62..da482d7fcf 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -120,7 +120,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index 11624bb479..87da8279ef 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -118,7 +118,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 6ff26f5023..a93567e1da 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -119,7 +119,6 @@ px4_add_board( #hello #hwtest # Hardware test #matlab_csv_serial - #position_estimator_inav #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html #rover_steering_control # Rover example app diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index eae18644ba..054b543564 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -120,7 +120,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/px4/fmu-v5x/rtps.cmake b/boards/px4/fmu-v5x/rtps.cmake index 25bad97554..975527927f 100644 --- a/boards/px4/fmu-v5x/rtps.cmake +++ b/boards/px4/fmu-v5x/rtps.cmake @@ -121,7 +121,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/px4/fmu-v5x/stackcheck.cmake b/boards/px4/fmu-v5x/stackcheck.cmake index 0855420907..8e1c010ec9 100644 --- a/boards/px4/fmu-v5x/stackcheck.cmake +++ b/boards/px4/fmu-v5x/stackcheck.cmake @@ -120,7 +120,6 @@ px4_add_board( #hello #hwtest # Hardware test #matlab_csv_serial - #position_estimator_inav #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html #rover_steering_control # Rover example app diff --git a/boards/px4/raspberrypi/cross.cmake b/boards/px4/raspberrypi/cross.cmake index 38d29ddde8..04c5a723a0 100644 --- a/boards/px4/raspberrypi/cross.cmake +++ b/boards/px4/raspberrypi/cross.cmake @@ -73,7 +73,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/px4/raspberrypi/native.cmake b/boards/px4/raspberrypi/native.cmake index f317e5e373..da7bb77090 100644 --- a/boards/px4/raspberrypi/native.cmake +++ b/boards/px4/raspberrypi/native.cmake @@ -72,7 +72,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 3c9c66564f..b7faf27a45 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -77,7 +77,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index a14e6c0d66..89d84b1563 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -78,7 +78,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index cdf693b794..1e270c641c 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -77,7 +77,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app diff --git a/src/examples/position_estimator_inav/CMakeLists.txt b/src/examples/position_estimator_inav/CMakeLists.txt deleted file mode 100644 index 4311ccd05d..0000000000 --- a/src/examples/position_estimator_inav/CMakeLists.txt +++ /dev/null @@ -1,47 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -px4_add_module( - MODULE modules__position_estimator_inav - MAIN position_estimator_inav - STACK_MAIN 1200 - STACK_MAX 4000 - COMPILE_FLAGS - SRCS - position_estimator_inav_main.cpp - inertial_filter.cpp - DEPENDS - terrain_estimation - git_ecl - ecl_geo - ) diff --git a/src/examples/position_estimator_inav/inertial_filter.cpp b/src/examples/position_estimator_inav/inertial_filter.cpp deleted file mode 100644 index 4f045aeb6b..0000000000 --- a/src/examples/position_estimator_inav/inertial_filter.cpp +++ /dev/null @@ -1,34 +0,0 @@ -/* - * inertial_filter.c - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - */ - -#include "px4_defines.h" -#include "inertial_filter.h" -#include - -void inertial_filter_predict(float dt, float x[2], float acc) -{ - if (PX4_ISFINITE(dt)) { - if (!PX4_ISFINITE(acc)) { - acc = 0.0f; - } - - x[0] += x[1] * dt + acc * dt * dt / 2.0f; - x[1] += acc * dt; - } -} - -void inertial_filter_correct(float e, float dt, float x[2], int i, float w) -{ - if (PX4_ISFINITE(e) && PX4_ISFINITE(w) && PX4_ISFINITE(dt)) { - float ewdt = e * w * dt; - x[i] += ewdt; - - if (i == 0) { - x[1] += w * ewdt; - } - } -} diff --git a/src/examples/position_estimator_inav/inertial_filter.h b/src/examples/position_estimator_inav/inertial_filter.h deleted file mode 100644 index 5f8b40ef14..0000000000 --- a/src/examples/position_estimator_inav/inertial_filter.h +++ /dev/null @@ -1,15 +0,0 @@ -/* - * inertial_filter.h - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - */ - -#pragma once - -#include -#include - -void inertial_filter_predict(float dt, float x[2], float acc); - -void inertial_filter_correct(float e, float dt, float x[2], int i, float w); diff --git a/src/examples/position_estimator_inav/params.c b/src/examples/position_estimator_inav/params.c deleted file mode 100644 index 2ae40b185e..0000000000 --- a/src/examples/position_estimator_inav/params.c +++ /dev/null @@ -1,346 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013, 2014 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 position_estimator_inav_params.c - * - * @author Anton Babushkin - * - * Parameters for position_estimator_inav - */ - -#include "position_estimator_inav_params.h" - -/** - * Z axis weight for barometer - * - * Weight (cutoff frequency) for barometer altitude measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); - -/** - * Z axis weight for GPS - * - * Weight (cutoff frequency) for GPS altitude measurements. GPS altitude data is very noisy and should be used only as slow correction for baro offset. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); - -/** - * Z velocity weight for GPS - * - * Weight (cutoff frequency) for GPS altitude velocity measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f); - -/** - * Z axis weight for vision - * - * Weight (cutoff frequency) for vision altitude measurements. vision altitude data is very noisy and should be used only as slow correction for baro offset. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 5.0f); - -/** - * Z axis weight for lidar - * - * Weight (cutoff frequency) for lidar measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_Z_LIDAR, 3.0f); - -/** - * XY axis weight for GPS position - * - * Weight (cutoff frequency) for GPS position measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); - -/** - * XY axis weight for GPS velocity - * - * Weight (cutoff frequency) for GPS velocity measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); - -/** - * XY axis weight for vision position - * - * Weight (cutoff frequency) for vision position measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 7.0f); - -/** - * XY axis weight for vision velocity - * - * Weight (cutoff frequency) for vision velocity measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f); - -/** - * Weight for mocap system - * - * Weight (cutoff frequency) for mocap position measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ - -PARAM_DEFINE_FLOAT(INAV_W_MOC_P, 10.0f); - -/** - * XY axis weight for optical flow - * - * Weight (cutoff frequency) for optical flow (velocity) measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 0.8f); - -/** - * XY axis weight for resetting velocity - * - * When velocity sources lost slowly decrease estimated horizontal velocity with this weight. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f); - -/** - * XY axis weight factor for GPS when optical flow available - * - * When optical flow data available, multiply GPS weights (for position and velocity) by this factor. - * - * @min 0.0 - * @max 1.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); - -/** - * Accelerometer bias estimation weight - * - * Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. - * - * @min 0.0 - * @max 0.1 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); - -/** - * Optical flow scale factor - * - * Factor to scale optical flow - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.35f); - -/** - * Minimal acceptable optical flow quality - * - * 0 - lowest quality, 1 - best quality. - * - * @min 0.0 - * @max 1.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.3f); - -/** - * Sonar maximal error for new surface - * - * If sonar measurement error is larger than this value it skiped (spike) or accepted as new surface level (if offset is stable). - * - * @min 0.0 - * @max 1.0 - * @unit m - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_LIDAR_ERR, 0.2f); - -/** - * Land detector time - * - * Vehicle assumed landed if no altitude changes happened during this time on low throttle. - * - * @min 0.0 - * @max 10.0 - * @unit s - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); - -/** - * Land detector altitude dispersion threshold - * - * Dispersion threshold for triggering land detector. - * - * @min 0.0 - * @max 10.0 - * @unit m - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); - -/** - * Land detector throttle threshold - * - * Value should be lower than minimal hovering thrust. Half of it is good choice. - * - * @min 0.0 - * @max 1.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); - -/** - * GPS delay - * - * GPS delay compensation - * - * @min 0.0 - * @max 1.0 - * @unit s - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); - -/** - * Flow module offset (center of rotation) in X direction - * - * Yaw X flow compensation - * - * @min -1.0 - * @max 1.0 - * @unit m - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f); - -/** - * Flow module offset (center of rotation) in Y direction - * - * Yaw Y flow compensation - * - * @min -1.0 - * @max 1.0 - * @unit m - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f); - -/** - * Mo-cap - * - * Set to 0 if using fake GPS - * - * @value 0 Mo-cap enabled - * @value 1 Mo-cap disabled - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_DISAB_MOCAP, 0); - -/** - * LIDAR for altitude estimation - * - * @boolean - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_LIDAR_EST, 0); - -/** - * LIDAR calibration offset - * - * LIDAR calibration offset. Value will be added to the measured distance - * - * @min -20 - * @max 20 - * @unit m - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_LIDAR_OFF, 0.0f); - -/** - * Disable vision input - * - * Set to the appropriate key (328754) to disable vision input. - * - * @reboot_required true - * @min 0 - * @max 328754 - * @category Developer - * @group Position Estimator INAV - */ -PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); diff --git a/src/examples/position_estimator_inav/position_estimator_inav_main.cpp b/src/examples/position_estimator_inav/position_estimator_inav_main.cpp deleted file mode 100644 index c7ff8c6e9a..0000000000 --- a/src/examples/position_estimator_inav/position_estimator_inav_main.cpp +++ /dev/null @@ -1,1599 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2018 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 position_estimator_inav_main.c - * Model-identification based position estimator for multirotors - * - * @author Anton Babushkin - * @author Nuno Marques - * @author Christoph Tobler - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include "position_estimator_inav_params.h" -#include "inertial_filter.h" - -#define MIN_VALID_W 0.00001f -#define PUB_INTERVAL 10000 // limit publish rate to 100 Hz -#define EST_BUF_SIZE 250000 / PUB_INTERVAL // buffer size is 0.5s -#define MAX_WAIT_FOR_BARO_SAMPLE 3000000 // wait 3 secs for the baro to respond - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int position_estimator_inav_task; /**< Handle of deamon task / thread */ -static bool inav_verbose_mode = false; - -static const hrt_abstime vision_topic_timeout = 500000; // Vision topic timeout = 0.5s -static const hrt_abstime mocap_topic_timeout = 500000; // Mocap topic timeout = 0.5s -static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s -static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s -static const hrt_abstime lidar_timeout = 150000; // lidar timeout = 150ms -static const hrt_abstime lidar_valid_timeout = 1000000; // estimate lidar distance during this time after lidar loss -static const unsigned updates_counter_len = 1000000; -static const float max_flow = 1.0f; // max flow value that can be used, rad/s - -extern "C" __EXPORT int position_estimator_inav_main(int argc, char *argv[]); - -int position_estimator_inav_thread_main(int argc, char *argv[]); - -static void usage(const char *reason); - -static inline int min(int val1, int val2) -{ - return (val1 < val2) ? val1 : val2; -} - -static inline int max(int val1, int val2) -{ - return (val1 > val2) ? val1 : val2; -} - -/** - * Print the correct usage. - */ -static void usage(const char *reason) -{ - if (reason && *reason) { - PX4_INFO("%s", reason); - } - - PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n"); -} - -/** - * The position_estimator_inav_thread 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 position_estimator_inav_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage("missing command"); - return -1; - } - - if (!strcmp(argv[1], "start")) { - if (thread_running) { - warnx("already running"); - /* this is not an error */ - return 0; - } - - inav_verbose_mode = false; - - if ((argc > 2) && (!strcmp(argv[2], "-v"))) { - inav_verbose_mode = true; - } - - thread_should_exit = false; - position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav", - SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4600, - position_estimator_inav_thread_main, - (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) nullptr); - 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; -} - -#ifdef INAV_DEBUG -static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], - float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], - float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v, float corr_mocap[3][1], float w_mocap_p, - float corr_vision[3][2], float w_xy_vision_p, float w_z_vision_p, float w_xy_vision_v) -{ - FILE *f = fopen(PX4_STORAGEDIR"/inav.log", "a"); - - if (f) { - char *s = malloc(256); - unsigned n = snprintf(s, 256, - "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", - (unsigned long long)hrt_absolute_time(), msg, (double)dt, - (double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1], - (double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], - (double)z_est_prev[1]); - fwrite(s, 1, n, f); - n = snprintf(s, 256, - "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f mocap_pos_corr=[%.5f %.5f %.5f] w_mocap_p=%.5f\n", - (double)acc[0], (double)acc[1], (double)acc[2], - (double)corr_gps[0][0], (double)corr_gps[1][0], (double)corr_gps[2][0], (double)corr_gps[0][1], (double)corr_gps[1][1], - (double)corr_gps[2][1], - (double)w_xy_gps_p, (double)w_xy_gps_v, (double)corr_mocap[0][0], (double)corr_mocap[1][0], (double)corr_mocap[2][0], - (double)w_mocap_p); - fwrite(s, 1, n, f); - n = snprintf(s, 256, - "\tvision_pos_corr=[%.5f %.5f %.5f] vision_vel_corr=[%.5f %.5f %.5f] w_xy_vision_p=%.5f w_z_vision_p=%.5f w_xy_vision_v=%.5f\n", - (double)corr_vision[0][0], (double)corr_vision[1][0], (double)corr_vision[2][0], (double)corr_vision[0][1], - (double)corr_vision[1][1], (double)corr_vision[2][1], - (double)w_xy_vision_p, (double)w_z_vision_p, (double)w_xy_vision_v); - fwrite(s, 1, n, f); - free(s); - } - - fsync(fileno(f)); - fclose(f); -} -#else -#define write_debug_log(...) -#endif - -/**************************************************************************** - * main - ****************************************************************************/ -int position_estimator_inav_thread_main(int argc, char *argv[]) -{ - orb_advert_t mavlink_log_pub = nullptr; - - float x_est[2] = { 0.0f, 0.0f }; // pos, vel - float y_est[2] = { 0.0f, 0.0f }; // pos, vel - float z_est[2] = { 0.0f, 0.0f }; // pos, vel - - float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer - float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer - float R_gps[3][3]; // rotation matrix for GPS correction moment - memset(est_buf, 0, sizeof(est_buf)); - memset(R_buf, 0, sizeof(R_buf)); - memset(R_gps, 0, sizeof(R_gps)); - int buf_ptr = 0; - - static const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation - static const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation - - float eph = max_eph_epv; - float epv = 1.0f; - - float eph_flow = 1.0f; - - float eph_vision = 0.2f; - float epv_vision = 0.2f; - - float eph_mocap = 0.05f; - float epv_mocap = 0.05f; - - float x_est_prev[2], y_est_prev[2], z_est_prev[2]; - memset(x_est_prev, 0, sizeof(x_est_prev)); - memset(y_est_prev, 0, sizeof(y_est_prev)); - memset(z_est_prev, 0, sizeof(z_est_prev)); - - int baro_init_cnt = 0; - int baro_init_num = 200; - float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted - - hrt_abstime accel_timestamp = 0; - hrt_abstime baro_timestamp = 0; - - bool ref_inited = false; - hrt_abstime ref_init_start = 0; - const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix - struct map_projection_reference_s ref; - memset(&ref, 0, sizeof(ref)); - - uint16_t accel_updates = 0; - uint16_t baro_updates = 0; - uint16_t gps_updates = 0; - uint16_t attitude_updates = 0; - uint16_t flow_updates = 0; - uint16_t vision_updates = 0; - uint16_t mocap_updates = 0; - - hrt_abstime updates_counter_start = hrt_absolute_time(); - hrt_abstime pub_last = hrt_absolute_time(); - - hrt_abstime t_prev = 0; - - /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ - float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D - float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame - float corr_baro = 0.0f; // D - float corr_gps[3][2] = { - { 0.0f, 0.0f }, // N (pos, vel) - { 0.0f, 0.0f }, // E (pos, vel) - { 0.0f, 0.0f }, // D (pos, vel) - }; - float w_gps_xy = 1.0f; - float w_gps_z = 1.0f; - - float corr_vision[3][2] = { - { 0.0f, 0.0f }, // N (pos, vel) - { 0.0f, 0.0f }, // E (pos, vel) - { 0.0f, 0.0f }, // D (pos, vel) - }; - - float corr_mocap[3][1] = { - { 0.0f }, // N (pos) - { 0.0f }, // E (pos) - { 0.0f }, // D (pos) - }; - const int mocap_heading = 2; - - float dist_ground = 0.0f; //variables for lidar altitude estimation - float corr_lidar = 0.0f; - float lidar_offset = 0.0f; - int lidar_offset_count = 0; - bool lidar_first = true; - bool use_lidar = false; - bool use_lidar_prev = false; - - float corr_flow[] = { 0.0f, 0.0f }; // N E - float w_flow = 0.0f; - - hrt_abstime lidar_time = 0; // time of last lidar measurement (not filtered) - hrt_abstime lidar_valid_time = 0; // time of last lidar measurement used for correction (filtered) - - int n_flow = 0; - float gyro_offset_filtered[] = { 0.0f, 0.0f, 0.0f }; - float flow_gyrospeed[] = { 0.0f, 0.0f, 0.0f }; - float flow_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f }; - float att_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f }; - float yaw_comp[] = { 0.0f, 0.0f }; - hrt_abstime flow_time = 0; - float flow_min_dist = 0.2f; - - bool gps_valid = false; // GPS is valid - bool lidar_valid = false; // lidar is valid - bool flow_valid = false; // flow is valid - bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) - bool vision_xy_valid = false; // vision XY is valid - bool vision_z_valid = false; // vision Z is valid - bool vision_vxy_valid = false; // vision VXY is valid - bool vision_vz_valid = false; // vision VZ is valid - bool mocap_xy_valid = false; // mocap XY is valid - bool mocap_z_valid = false; // mocap Z is valid - - /* set pose/velocity as invalid if standard deviation is bigger than max_std_dev */ - /* TODO: the user should be allowed to set these values by a parameter */ - static constexpr float ep_max_std_dev = 100.0f; // position estimation max std deviation - static constexpr float ev_max_std_dev = 100.0f; // velocity estimation max std deviation - - /* declare and safely initialize all structs */ - struct actuator_controls_s actuator; - memset(&actuator, 0, sizeof(actuator)); - struct actuator_armed_s armed; - memset(&armed, 0, sizeof(armed)); - struct sensor_combined_s sensor; - memset(&sensor, 0, sizeof(sensor)); - struct vehicle_gps_position_s gps; - memset(&gps, 0, sizeof(gps)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_local_position_s pos; - memset(&pos, 0, sizeof(pos)); - struct optical_flow_s flow; - memset(&flow, 0, sizeof(flow)); - struct vehicle_odometry_s visual_odom; - memset(&visual_odom, 0, sizeof(visual_odom)); - struct vehicle_odometry_s mocap; - memset(&mocap, 0, sizeof(mocap)); - struct vehicle_global_position_s global_pos; - memset(&global_pos, 0, sizeof(global_pos)); - struct distance_sensor_s lidar; - memset(&lidar, 0, sizeof(lidar)); - struct vehicle_rates_setpoint_s rates_setpoint; - memset(&rates_setpoint, 0, sizeof(rates_setpoint)); - struct vehicle_air_data_s airdata; - memset(&airdata, 0, sizeof(vehicle_air_data_s)); - - /* subscribe */ - int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); - int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - int visual_odom_sub = orb_subscribe(ORB_ID(vehicle_visual_odometry)); - int mocap_position_sub = orb_subscribe(ORB_ID(vehicle_mocap_odometry)); - int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - int vehicle_air_data_sub = orb_subscribe(ORB_ID(vehicle_air_data)); - // because we can have several distance sensor instances with different orientations - int distance_sensor_subs[ORB_MULTI_MAX_INSTANCES]; - - for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - distance_sensor_subs[i] = orb_subscribe_multi(ORB_ID(distance_sensor), i); - } - - /* advertise */ - orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &pos); - orb_advert_t vehicle_global_position_pub = nullptr; - - struct position_estimator_inav_params params; - memset(¶ms, 0, sizeof(params)); - struct position_estimator_inav_param_handles pos_inav_param_handles; - /* initialize parameter handles */ - inav_parameters_init(&pos_inav_param_handles); - - /* first parameters read at start up */ - struct parameter_update_s param_update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, - ¶m_update); /* read from param topic to clear updated flag */ - /* first parameters update */ - inav_parameters_update(&pos_inav_param_handles, ¶ms); - - px4_pollfd_struct_t fds_init[1] = {}; - fds_init[0].fd = sensor_combined_sub; - fds_init[0].events = POLLIN; - - /* wait for initial baro value */ - bool wait_baro = true; - TerrainEstimator terrain_estimator; - - thread_running = true; - hrt_abstime baro_wait_for_sample_time = hrt_absolute_time(); - - while (wait_baro && !thread_should_exit) { - int ret = px4_poll(&fds_init[0], 1, 1000); - - if (ret < 0) { - /* poll error */ - mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init"); - - } else if (hrt_absolute_time() - baro_wait_for_sample_time > MAX_WAIT_FOR_BARO_SAMPLE) { - wait_baro = false; - mavlink_log_info(&mavlink_log_pub, "[inav] timed out waiting for a baro sample"); - - } else if (ret > 0) { - if (fds_init[0].revents & POLLIN) { - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - - bool baro_updated = false; - orb_check(vehicle_air_data_sub, &baro_updated); - - if (baro_updated) { - orb_copy(ORB_ID(vehicle_air_data), vehicle_air_data_sub, &airdata); - - if (wait_baro && airdata.timestamp != baro_timestamp) { - - baro_timestamp = airdata.timestamp; - baro_wait_for_sample_time = hrt_absolute_time(); - - /* mean calculation over several measurements */ - if (baro_init_cnt < baro_init_num) { - if (PX4_ISFINITE(airdata.baro_alt_meter)) { - baro_offset += airdata.baro_alt_meter; - baro_init_cnt++; - } - - } else { - wait_baro = false; - baro_offset /= (float) baro_init_cnt; - pos.z_valid = true; - pos.v_z_valid = true; - } - } - } - - - } - - } else { - PX4_WARN("INAV poll timeout"); - } - } - - /* main loop */ - px4_pollfd_struct_t fds[1]; - fds[0].fd = vehicle_attitude_sub; - fds[0].events = POLLIN; - - while (!thread_should_exit) { - int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate - hrt_abstime t = hrt_absolute_time(); - - if (ret < 0) { - /* poll error */ - mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init"); - continue; - - } else if (ret > 0) { - /* act on attitude updates */ - - /* vehicle attitude */ - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - attitude_updates++; - - bool updated; - - /* parameter update */ - orb_check(parameter_update_sub, &updated); - - if (updated) { - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - inav_parameters_update(&pos_inav_param_handles, ¶ms); - } - - /* actuator */ - orb_check(actuator_sub, &updated); - - if (updated) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator); - } - - /* armed */ - orb_check(armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - } - - /* sensor combined */ - orb_check(sensor_combined_sub, &updated); - - matrix::Dcmf R = matrix::Quatf(att.q); - - if (updated) { - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - - if (sensor.timestamp + sensor.accelerometer_timestamp_relative != accel_timestamp) { - /* correct accel bias */ - sensor.accelerometer_m_s2[0] -= acc_bias[0]; - sensor.accelerometer_m_s2[1] -= acc_bias[1]; - sensor.accelerometer_m_s2[2] -= acc_bias[2]; - - /* transform acceleration vector from body frame to NED frame */ - for (int i = 0; i < 3; i++) { - acc[i] = 0.0f; - - for (int j = 0; j < 3; j++) { - acc[i] += R(i, j) * sensor.accelerometer_m_s2[j]; - } - } - - acc[2] += CONSTANTS_ONE_G; - - accel_timestamp = sensor.timestamp + sensor.accelerometer_timestamp_relative; - accel_updates++; - } - - if (airdata.timestamp != baro_timestamp) { - corr_baro = baro_offset - airdata.baro_alt_meter - z_est[0]; - baro_timestamp = airdata.timestamp; - baro_updates++; - } - } - - - /* lidar alt estimation - * update lidar separately, needed by terrain estimator */ - for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - - orb_check(distance_sensor_subs[i], &updated); - - if (updated) { - - orb_copy(ORB_ID(distance_sensor), distance_sensor_subs[i], &lidar); - - if (lidar.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) { - updated = false; - - } else { - lidar.current_distance += params.lidar_calibration_offset; - break; // only the first valid distance sensor instance is used - } - } - } - - if (updated) { //check if altitude estimation for lidar is enabled and new sensor data - - if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance - && lidar.current_distance < lidar.max_distance - && (R(2, 2) > 0.7f)) { - - if (!use_lidar_prev && use_lidar) { - lidar_first = true; - } - - use_lidar_prev = use_lidar; - - lidar_time = t; - dist_ground = lidar.current_distance * R(2, 2); //vertical distance - - if (lidar_first) { - lidar_first = false; - lidar_offset = dist_ground + z_est[0]; - mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR: new ground offset"); - warnx("[inav] LIDAR: new ground offset"); - } - - corr_lidar = lidar_offset - dist_ground - z_est[0]; - - if (fabsf(corr_lidar) > params.lidar_err) { //check for spike - corr_lidar = 0; - lidar_valid = false; - lidar_offset_count++; - - if (lidar_offset_count > 3) { //if consecutive bigger/smaller measurements -> new ground offset -> reinit - lidar_first = true; - lidar_offset_count = 0; - } - - } else { - corr_lidar = lidar_offset - dist_ground - z_est[0]; - lidar_valid = true; - lidar_offset_count = 0; - lidar_valid_time = t; - } - - } else { - lidar_valid = false; - } - } - - /* optical flow */ - orb_check(optical_flow_sub, &updated); - - if (updated && lidar_valid) { - orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); - - flow_time = t; - float flow_q = flow.quality / 255.0f; - float dist_bottom = lidar.current_distance; - - if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && R(2, 2) > 0.7f) { - /* distance to surface */ - //float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar - float flow_dist = dist_bottom; //use this if using lidar - - /* check if flow if too large for accurate measurements */ - /* calculate estimated velocity in body frame */ - float body_v_est[2] = { 0.0f, 0.0f }; - - for (int i = 0; i < 2; i++) { - body_v_est[i] = R(0, i) * x_est[1] + R(1, i) * y_est[1] + R(2, i) * z_est[1]; - } - - /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ - flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && - fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; - - /*calculate offset of flow-gyro using already calibrated gyro from autopilot*/ - flow_gyrospeed[0] = flow.gyro_x_rate_integral / (float)flow.integration_timespan * 1000000.0f; - flow_gyrospeed[1] = flow.gyro_y_rate_integral / (float)flow.integration_timespan * 1000000.0f; - flow_gyrospeed[2] = flow.gyro_z_rate_integral / (float)flow.integration_timespan * 1000000.0f; - - //moving average - if (n_flow >= 100) { - gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0]; - gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1]; - gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2]; - n_flow = 0; - flow_gyrospeed_filtered[0] = 0.0f; - flow_gyrospeed_filtered[1] = 0.0f; - flow_gyrospeed_filtered[2] = 0.0f; - att_gyrospeed_filtered[0] = 0.0f; - att_gyrospeed_filtered[1] = 0.0f; - att_gyrospeed_filtered[2] = 0.0f; - - } else { - flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1); - flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1); - flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1); - att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1); - att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1); - att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1); - n_flow++; - } - - - /*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/ - yaw_comp[0] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]); - yaw_comp[1] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]); - - /* - * Convert raw flow from the optical_flow uORB topic (rad) to angular flow (rad/s) - * Note that the optical_flow uORB topic defines positive delta angles as produced by RH rotations - * around the correspdonding body axes. - */ - - float flow_ang[2]; - - /* check for vehicle rates setpoint - it below threshold -> dont subtract -> better hover */ - orb_check(vehicle_rate_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint); - } - - float rate_threshold = 0.15f; - - /* calculate the angular flow rate produced by a negative velocity along the X body axis */ - if (fabsf(rates_setpoint.pitch) < rate_threshold) { - //warnx("[inav] test ohne comp"); - flow_ang[0] = (-flow.pixel_flow_y_integral / (float)flow.integration_timespan * 1000000.0f) * - params.flow_k;//for now the flow has to be scaled (to small) - - } else { - //warnx("[inav] test mit comp"); - //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro) - flow_ang[0] = (-(flow.pixel_flow_y_integral - flow.gyro_y_rate_integral) / (float)flow.integration_timespan * 1000000.0f - + gyro_offset_filtered[0]) * params.flow_k;//for now the flow has to be scaled (to small) - } - - /* calculate the angular flow rate produced by a negative velocity along the Y body axis */ - if (fabsf(rates_setpoint.roll) < rate_threshold) { - flow_ang[1] = (flow.pixel_flow_x_integral / (float)flow.integration_timespan * 1000000.0f) * - params.flow_k;//for now the flow has to be scaled (to small) - - } else { - //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro) - flow_ang[1] = ((flow.pixel_flow_x_integral - flow.gyro_x_rate_integral) / (float)flow.integration_timespan * 1000000.0f - + gyro_offset_filtered[1]) * params.flow_k;//for now the flow has to be scaled (to small) - } - - /* flow measurements vector */ - float flow_m[3]; - - if (fabsf(rates_setpoint.yaw) < rate_threshold) { - flow_m[0] = -flow_ang[0] * flow_dist; - flow_m[1] = -flow_ang[1] * flow_dist; - - } else { - flow_m[0] = -flow_ang[0] * flow_dist - yaw_comp[0] * params.flow_k; - flow_m[1] = -flow_ang[1] * flow_dist - yaw_comp[1] * params.flow_k; - } - - flow_m[2] = z_est[1]; - - /* velocity in NED */ - float flow_v[2] = { 0.0f, 0.0f }; - - /* project measurements vector to NED basis, skip Z component */ - for (int i = 0; i < 2; i++) { - for (int j = 0; j < 3; j++) { - flow_v[i] += R(i, j) * flow_m[j]; - } - } - - /* velocity correction */ - corr_flow[0] = flow_v[0] - x_est[1]; - corr_flow[1] = flow_v[1] - y_est[1]; - /* adjust correction weight */ - float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); - w_flow = R(2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist); - - - /* if flow is not accurate, reduce weight for it */ - // TODO make this more fuzzy - if (!flow_accurate) { - w_flow *= 0.05f; - } - - /* under ideal conditions, on 1m distance assume EPH = 10cm */ - eph_flow = 0.1f / w_flow; - - flow_valid = true; - - } else { - w_flow = 0.0f; - flow_valid = false; - } - - flow_updates++; - } - - /* check no vision circuit breaker is set */ - if (params.no_vision != CBRK_NO_VISION_KEY) { - /* vehicle visual odometry */ - orb_check(visual_odom_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_visual_odometry), visual_odom_sub, &visual_odom); - - static float last_vision_x = 0.0f; - static float last_vision_y = 0.0f; - static float last_vision_z = 0.0f; - - vision_xy_valid = PX4_ISFINITE(visual_odom.x) - && (PX4_ISFINITE(visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE]) ? sqrtf(fmaxf( - visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE], - visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_Y_VARIANCE])) <= ep_max_std_dev : true); - vision_z_valid = PX4_ISFINITE(visual_odom.z) - && (PX4_ISFINITE(visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE]) ? - visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_Z_VARIANCE] <= ep_max_std_dev : true); - vision_vxy_valid = PX4_ISFINITE(visual_odom.vx) - && PX4_ISFINITE(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE]) ? sqrtf( - fmaxf(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE], - visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VY_VARIANCE])) <= ev_max_std_dev : true; - vision_vz_valid = PX4_ISFINITE(visual_odom.vz) - && (PX4_ISFINITE(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE]) ? - visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VZ_VARIANCE] <= ep_max_std_dev : true); - - /* reset position estimate on first vision update */ - if (vision_xy_valid) { - x_est[0] = visual_odom.x; - y_est[0] = visual_odom.y; - - last_vision_x = visual_odom.x; - last_vision_y = visual_odom.y; - - } else { - warnx("VISION XY estimate not valid"); - mavlink_log_info(&mavlink_log_pub, "[inav] VISION XY estimate not valid"); - - } - - if (vision_vxy_valid) { - x_est[1] = visual_odom.vx; - y_est[1] = visual_odom.vy; - - } else { - warnx("VISION VXY estimate not valid"); - mavlink_log_info(&mavlink_log_pub, "[inav] VISION VXY estimate not valid"); - - } - - /* only reset the z estimate if the z weight parameter is not zero */ - if (params.w_z_vision_p > MIN_VALID_W) { - if (vision_z_valid) { - z_est[0] = visual_odom.z; - - last_vision_z = visual_odom.z; - - } else { - warnx("VISION Z estimate not valid"); - mavlink_log_info(&mavlink_log_pub, "[inav] VISION Z estimate not valid"); - - } - - if (vision_vz_valid) { - z_est[1] = visual_odom.vz; - - } else { - warnx("VISION VZ estimate not valid"); - mavlink_log_info(&mavlink_log_pub, "[inav] VISION VZ estimate not valid"); - - } - } - - /* calculate correction for position */ - if (vision_xy_valid) { - corr_vision[0][0] = visual_odom.x - x_est[0]; - corr_vision[1][0] = visual_odom.y - y_est[0]; - } - - if (vision_z_valid) { - corr_vision[2][0] = visual_odom.z - z_est[0]; - } - - static hrt_abstime last_vision_time = 0; - - float vision_dt = (visual_odom.timestamp - last_vision_time) / 1e6f; - last_vision_time = visual_odom.timestamp; - - if (vision_vxy_valid) { - /* calculate correction for XY velocity from external estimation */ - corr_vision[0][1] = visual_odom.vx - x_est[1]; - corr_vision[1][1] = visual_odom.vy - y_est[1]; - - } else if (vision_dt > 0.000001f && vision_dt < 0.2f && vision_xy_valid) { - visual_odom.vx = (visual_odom.x - last_vision_x) / vision_dt; - visual_odom.vy = (visual_odom.y - last_vision_y) / vision_dt; - - last_vision_x = visual_odom.x; - last_vision_y = visual_odom.y; - - /* calculate correction for XY velocity */ - corr_vision[0][1] = visual_odom.vx - x_est[1]; - corr_vision[1][1] = visual_odom.vy - y_est[1]; - - } else { - /* assume zero motion in XY plane */ - corr_vision[0][1] = 0.0f - x_est[1]; - corr_vision[1][1] = 0.0f - y_est[1]; - - } - - if (vision_vz_valid) { - /* calculate correction for Z velocity from external estimation */ - corr_vision[2][1] = visual_odom.vz - z_est[1]; - - } else if (vision_dt > 0.000001f && vision_dt < 0.2f && vision_z_valid) { - visual_odom.vz = (visual_odom.z - last_vision_z) / vision_dt; - - last_vision_z = visual_odom.z; - - /* calculate correction for Z velocity */ - corr_vision[2][1] = visual_odom.vz - z_est[1]; - - } else { - /* assume zero motion in Z plane */ - corr_vision[2][1] = 0.0f - z_est[1]; - } - - vision_updates++; - } - } - - /* vehicle mocap position */ - orb_check(mocap_position_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_mocap_odometry), mocap_position_sub, &mocap); - - mocap_xy_valid = PX4_ISFINITE(mocap.x) - && (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE]) ? sqrtf(fmaxf( - mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE], - mocap.pose_covariance[mocap.COVARIANCE_MATRIX_Y_VARIANCE])) <= ep_max_std_dev : true); - mocap_z_valid = PX4_ISFINITE(mocap.z) - && (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE]) ? - mocap.pose_covariance[mocap.COVARIANCE_MATRIX_Z_VARIANCE] <= ep_max_std_dev : true); - - if (!params.disable_mocap) { - /* reset position estimate on first mocap update */ - if (mocap_xy_valid) { - x_est[0] = mocap.x; - y_est[0] = mocap.y; - } - - if (mocap_z_valid) { - z_est[0] = mocap.z; - } - - if (!mocap_xy_valid || !mocap_z_valid) { - warnx("MOCAP data not valid"); - mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP data not valid");; - - } - - /* calculate correction for position */ - if (mocap_xy_valid) { - corr_mocap[0][0] = mocap.x - x_est[0]; - corr_mocap[1][0] = mocap.y - y_est[0]; - } - - if (mocap_z_valid) { - corr_mocap[2][0] = mocap.z - z_est[0]; - } - - mocap_updates++; - } - } - - /* vehicle GPS position */ - orb_check(vehicle_gps_position_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - - bool reset_est = false; - - /* hysteresis for GPS quality */ - if (gps_valid) { - if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) { - gps_valid = false; - mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal lost"); - warnx("[inav] GPS signal lost"); - } - - } else { - if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) { - gps_valid = true; - reset_est = true; - mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal found"); - warnx("[inav] GPS signal found"); - } - } - - if (gps_valid) { - double lat = gps.lat * 1e-7; - double lon = gps.lon * 1e-7; - float alt = gps.alt * 1e-3; - - /* initialize reference position if needed */ - if (!ref_inited) { - if (ref_init_start == 0) { - ref_init_start = t; - - } else if (t > ref_init_start + ref_init_delay) { - ref_inited = true; - - /* set position estimate to (0, 0, 0), use GPS velocity for XY */ - x_est[0] = 0.0f; - x_est[1] = gps.vel_n_m_s; - y_est[0] = 0.0f; - y_est[1] = gps.vel_e_m_s; - - pos.ref_lat = lat; - pos.ref_lon = lon; - pos.ref_alt = alt + z_est[0]; - pos.ref_timestamp = t; - - /* initialize projection */ - map_projection_init(&ref, lat, lon); - // XXX replace this print - warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt); - mavlink_log_info(&mavlink_log_pub, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); - } - } - - if (ref_inited) { - /* project GPS lat lon to plane */ - float gps_proj[2]; - map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); - - /* reset position estimate when GPS becomes good */ - if (reset_est) { - x_est[0] = gps_proj[0]; - x_est[1] = gps.vel_n_m_s; - y_est[0] = gps_proj[1]; - y_est[1] = gps.vel_e_m_s; - } - - /* calculate index of estimated values in buffer */ - int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL))); - - if (est_i < 0) { - est_i += EST_BUF_SIZE; - } - - /* calculate correction for position */ - corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0]; - corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0]; - corr_gps[2][0] = pos.ref_alt - alt - est_buf[est_i][2][0]; - - /* calculate correction for velocity */ - if (gps.vel_ned_valid) { - corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1]; - corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1]; - corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1]; - - } else { - corr_gps[0][1] = 0.0f; - corr_gps[1][1] = 0.0f; - corr_gps[2][1] = 0.0f; - } - - /* save rotation matrix at this moment */ - memcpy(R_gps, R_buf[est_i], sizeof(R_gps)); - - w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph); - w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv); - } - - } else { - /* no GPS lock */ - memset(corr_gps, 0, sizeof(corr_gps)); - ref_init_start = 0; - } - - gps_updates++; - } - } - - matrix::Dcm R = matrix::Quatf(att.q); - - /* check for timeout on FLOW topic */ - if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout)) { - flow_valid = false; - warnx("FLOW timeout"); - mavlink_log_info(&mavlink_log_pub, "[inav] FLOW timeout"); - } - - /* check for timeout on GPS topic */ - if (gps_valid && (t > (gps.timestamp + gps_topic_timeout))) { - gps_valid = false; - warnx("GPS timeout"); - mavlink_log_info(&mavlink_log_pub, "[inav] GPS timeout"); - } - - /* check for timeout on vision topic */ - if ((vision_xy_valid || vision_z_valid) && (t > (visual_odom.timestamp + vision_topic_timeout))) { - vision_xy_valid = false; - vision_z_valid = false; - warnx("VISION timeout"); - mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout"); - } - - /* check for timeout on mocap topic */ - if ((mocap_xy_valid || mocap_z_valid) && (t > (mocap.timestamp + mocap_topic_timeout))) { - mocap_xy_valid = false; - mocap_z_valid = false; - warnx("MOCAP timeout"); - mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP timeout"); - } - - /* check for lidar measurement timeout */ - if (lidar_valid && (t > (lidar_time + lidar_timeout))) { - lidar_valid = false; - warnx("LIDAR timeout"); - mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR timeout"); - } - - float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; - dt = fmaxf(fminf(0.02, dt), 0.0002); // constrain dt from 0.2 to 20 ms - t_prev = t; - - /* increase EPH/EPV on each step */ - if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0 - eph = 0.001; - } - - if (eph < max_eph_epv) { - eph *= 1.0f + dt; - } - - if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0 - epv = 0.001; - } - - if (epv < max_eph_epv) { - epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift) - } - - /* use GPS if it's valid and reference position initialized */ - bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; - bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; - /* use VISION if it's valid and has a valid weight parameter */ - bool use_vision_xy = vision_xy_valid && params.w_xy_vision_p > MIN_VALID_W; - bool use_vision_z = vision_z_valid && params.w_z_vision_p > MIN_VALID_W; - /* use MOCAP if it's valid and has a valid weight parameter */ - bool use_mocap = mocap_xy_valid && mocap_z_valid && params.w_mocap_p > MIN_VALID_W - && params.att_ext_hdg_m == mocap_heading; //check if external heading is mocap - - if (params.disable_mocap) { //disable mocap if fake gps is used - use_mocap = false; - } - - /* use flow if it's valid and (accurate or no GPS available) */ - bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); - - /* use LIDAR if it's valid and lidar altitude estimation is enabled */ - use_lidar = lidar_valid && params.enable_lidar_alt_est; - - bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; - - bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout); - - float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; - float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; - float w_z_gps_p = params.w_z_gps_p * w_gps_z; - float w_z_gps_v = params.w_z_gps_v * w_gps_z; - - float w_xy_vision_p = params.w_xy_vision_p; - float w_xy_vision_v = params.w_xy_vision_v; - float w_z_vision_p = params.w_z_vision_p; - - float w_mocap_p = params.w_mocap_p; - - /* reduce GPS weight if optical flow is good */ - if (use_flow && flow_accurate) { - w_xy_gps_p *= params.w_gps_flow; - w_xy_gps_v *= params.w_gps_flow; - } - - /* baro offset correction */ - if (use_gps_z) { - float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; - baro_offset += offs_corr; - corr_baro += offs_corr; - } - - /* accelerometer bias correction for GPS (use buffered rotation matrix) */ - float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; - - if (use_gps_xy) { - accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p; - accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v; - accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p; - accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v; - } - - if (use_gps_z) { - accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; - accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v; - } - - /* transform error vector from NED frame to body frame */ - for (int i = 0; i < 3; i++) { - float c = 0.0f; - - for (int j = 0; j < 3; j++) { - c += R_gps[j][i] * accel_bias_corr[j]; - } - - if (PX4_ISFINITE(c)) { - acc_bias[i] += c * params.w_acc_bias * dt; - } - } - - /* accelerometer bias correction for VISION (use buffered rotation matrix) */ - accel_bias_corr[0] = 0.0f; - accel_bias_corr[1] = 0.0f; - accel_bias_corr[2] = 0.0f; - - if (use_vision_xy) { - accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p; - accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v; - accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p; - accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v; - } - - if (use_vision_z) { - accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p; - } - - /* accelerometer bias correction for MOCAP (use buffered rotation matrix) */ - accel_bias_corr[0] = 0.0f; - accel_bias_corr[1] = 0.0f; - accel_bias_corr[2] = 0.0f; - - if (use_mocap) { - accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p; - accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p; - accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p; - } - - /* transform error vector from NED frame to body frame */ - for (int i = 0; i < 3; i++) { - float c = 0.0f; - - for (int j = 0; j < 3; j++) { - c += R(j, i) * accel_bias_corr[j]; - } - - if (PX4_ISFINITE(c)) { - acc_bias[i] += c * params.w_acc_bias * dt; - } - } - - /* accelerometer bias correction for flow and baro (assume that there is no delay) */ - accel_bias_corr[0] = 0.0f; - accel_bias_corr[1] = 0.0f; - accel_bias_corr[2] = 0.0f; - - if (use_flow) { - accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow; - accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow; - } - - if (use_lidar) { - accel_bias_corr[2] -= corr_lidar * params.w_z_lidar * params.w_z_lidar; - - } else { - accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro; - } - - /* transform error vector from NED frame to body frame */ - for (int i = 0; i < 3; i++) { - float c = 0.0f; - - for (int j = 0; j < 3; j++) { - c += R(j, i) * accel_bias_corr[j]; - } - - if (PX4_ISFINITE(c)) { - acc_bias[i] += c * params.w_acc_bias * dt; - } - } - - /* inertial filter prediction for altitude */ - inertial_filter_predict(dt, z_est, acc[2]); - - if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { - write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, - acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, - corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); - memcpy(z_est, z_est_prev, sizeof(z_est)); - } - - /* inertial filter correction for altitude */ - if (use_lidar) { - inertial_filter_correct(corr_lidar, dt, z_est, 0, params.w_z_lidar); - - } else { - inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); - } - - if (use_gps_z) { - epv = fminf(epv, gps.epv); - - inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); - inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); - } - - if (use_vision_z) { - epv = fminf(epv, epv_vision); - inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p); - } - - if (use_mocap) { - epv = fminf(epv, epv_mocap); - inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p); - } - - if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { - write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, - acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, - corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); - memcpy(z_est, z_est_prev, sizeof(z_est)); - memset(corr_gps, 0, sizeof(corr_gps)); - memset(corr_vision, 0, sizeof(corr_vision)); - memset(corr_mocap, 0, sizeof(corr_mocap)); - corr_baro = 0; - - } else { - memcpy(z_est_prev, z_est, sizeof(z_est)); - } - - if (can_estimate_xy) { - /* inertial filter prediction for position */ - inertial_filter_predict(dt, x_est, acc[0]); - inertial_filter_predict(dt, y_est, acc[1]); - - if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { - write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, - acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, - corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); - memcpy(x_est, x_est_prev, sizeof(x_est)); - memcpy(y_est, y_est_prev, sizeof(y_est)); - } - - /* inertial filter correction for position */ - if (use_flow) { - eph = fminf(eph, eph_flow); - - inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); - inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); - } - - if (use_gps_xy) { - eph = fminf(eph, gps.eph); - - inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p); - inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p); - - if (gps.vel_ned_valid && t < gps.timestamp + gps_topic_timeout) { - inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v); - inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v); - } - } - - if (use_vision_xy) { - eph = fminf(eph, eph_vision); - - inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p); - inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p); - - if (w_xy_vision_v > MIN_VALID_W) { - inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v); - inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v); - } - } - - if (use_mocap) { - eph = fminf(eph, eph_mocap); - - inertial_filter_correct(corr_mocap[0][0], dt, x_est, 0, w_mocap_p); - inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p); - } - - if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { - write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, - acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, - corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); - memcpy(x_est, x_est_prev, sizeof(x_est)); - memcpy(y_est, y_est_prev, sizeof(y_est)); - memset(corr_gps, 0, sizeof(corr_gps)); - memset(corr_vision, 0, sizeof(corr_vision)); - memset(corr_mocap, 0, sizeof(corr_mocap)); - memset(corr_flow, 0, sizeof(corr_flow)); - - } else { - memcpy(x_est_prev, x_est, sizeof(x_est)); - memcpy(y_est_prev, y_est, sizeof(y_est)); - } - - } else { - /* gradually reset xy velocity estimates */ - inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v); - inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); - } - - /* run terrain estimator */ - terrain_estimator.predict(dt, &att, &sensor, &lidar); - terrain_estimator.measurement_update(hrt_absolute_time(), &gps, &lidar, &att); - - if (inav_verbose_mode) { - /* print updates rate */ - if (t > updates_counter_start + updates_counter_len) { - float updates_dt = (t - updates_counter_start) * 0.000001f; - warnx( - "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s, vision = %.1f/s, mocap = %.1f/s", - (double)(accel_updates / updates_dt), - (double)(baro_updates / updates_dt), - (double)(gps_updates / updates_dt), - (double)(attitude_updates / updates_dt), - (double)(flow_updates / updates_dt), - (double)(vision_updates / updates_dt), - (double)(mocap_updates / updates_dt)); - updates_counter_start = t; - accel_updates = 0; - baro_updates = 0; - gps_updates = 0; - attitude_updates = 0; - flow_updates = 0; - vision_updates = 0; - mocap_updates = 0; - } - } - - if (t > pub_last + PUB_INTERVAL) { - pub_last = t; - - /* push current estimate to buffer */ - est_buf[buf_ptr][0][0] = x_est[0]; - est_buf[buf_ptr][0][1] = x_est[1]; - est_buf[buf_ptr][1][0] = y_est[0]; - est_buf[buf_ptr][1][1] = y_est[1]; - est_buf[buf_ptr][2][0] = z_est[0]; - est_buf[buf_ptr][2][1] = z_est[1]; - - /* push current rotation matrix to buffer */ - memcpy(R_buf[buf_ptr], &R._data[0][0], sizeof(R._data)); - - buf_ptr++; - - if (buf_ptr >= EST_BUF_SIZE) { - buf_ptr = 0; - } - - - /* publish local position */ - pos.xy_valid = can_estimate_xy; - pos.v_xy_valid = can_estimate_xy; - pos.xy_global = pos.xy_valid && use_gps_xy; - pos.z_global = pos.z_valid && use_gps_z; - pos.x = x_est[0]; - pos.vx = x_est[1]; - pos.y = y_est[0]; - pos.vy = y_est[1]; - pos.z = z_est[0]; - pos.vz = z_est[1]; - pos.ax = NAN; - pos.ay = NAN; - pos.az = NAN; - pos.yaw = matrix::Eulerf(matrix::Quatf(att.q)).psi(); - pos.dist_bottom_valid = dist_bottom_valid; - pos.eph = eph; - pos.epv = epv; - pos.evh = 0.0f; - pos.evv = 0.0f; - pos.vxy_max = INFINITY; - pos.vz_max = INFINITY; - pos.hagl_min = INFINITY; - pos.hagl_max = INFINITY; - - // this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity - pos.z_deriv = z_est[1]; - - if (pos.dist_bottom_valid) { - pos.dist_bottom = dist_ground; - pos.dist_bottom_rate = - z_est[1]; - } - - pos.timestamp = t; - - orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos); - - if (pos.xy_global && pos.z_global) { - /* publish global position */ - global_pos.timestamp = t; - - double est_lat, est_lon; - map_projection_reproject(&ref, pos.x, pos.y, &est_lat, &est_lon); - - global_pos.lat = est_lat; - global_pos.lon = est_lon; - global_pos.alt = pos.ref_alt - pos.z; - - global_pos.vel_n = pos.vx; - global_pos.vel_e = pos.vy; - global_pos.vel_d = pos.vz; - - global_pos.yaw = matrix::Eulerf(matrix::Quatf(R)).psi(); - - global_pos.eph = eph; - global_pos.epv = epv; - - if (terrain_estimator.is_valid()) { - global_pos.terrain_alt = global_pos.alt - terrain_estimator.get_distance_to_ground(); - global_pos.terrain_alt_valid = true; - - } else { - global_pos.terrain_alt_valid = false; - } - - if (vehicle_global_position_pub == nullptr) { - vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); - - } else { - orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); - } - } - } - } - - warnx("stopped"); - mavlink_log_info(&mavlink_log_pub, "[inav] stopped"); - thread_running = false; - return 0; -} - - -int inav_parameters_init(struct position_estimator_inav_param_handles *h) -{ - h->w_z_baro = param_find("INAV_W_Z_BARO"); - h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); - h->w_z_gps_v = param_find("INAV_W_Z_GPS_V"); - h->w_z_vision_p = param_find("INAV_W_Z_VIS_P"); - h->w_z_lidar = param_find("INAV_W_Z_LIDAR"); - h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); - h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); - h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P"); - h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V"); - h->w_mocap_p = param_find("INAV_W_MOC_P"); - h->w_xy_flow = param_find("INAV_W_XY_FLOW"); - h->w_xy_res_v = param_find("INAV_W_XY_RES_V"); - h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); - h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); - h->flow_k = param_find("INAV_FLOW_K"); - h->flow_q_min = param_find("INAV_FLOW_Q_MIN"); - h->lidar_err = param_find("INAV_LIDAR_ERR"); - h->land_t = param_find("INAV_LAND_T"); - h->land_disp = param_find("INAV_LAND_DISP"); - h->land_thr = param_find("INAV_LAND_THR"); - h->no_vision = param_find("CBRK_NO_VISION"); - h->delay_gps = param_find("INAV_DELAY_GPS"); - h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X"); - h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y"); - h->disable_mocap = param_find("INAV_DISAB_MOCAP"); - h->enable_lidar_alt_est = param_find("INAV_LIDAR_EST"); - h->lidar_calibration_offset = param_find("INAV_LIDAR_OFF"); - h->att_ext_hdg_m = param_find("ATT_EXT_HDG_M"); - - return 0; -} - -int inav_parameters_update(const struct position_estimator_inav_param_handles *h, - struct position_estimator_inav_params *p) -{ - param_get(h->w_z_baro, &(p->w_z_baro)); - param_get(h->w_z_gps_p, &(p->w_z_gps_p)); - param_get(h->w_z_gps_v, &(p->w_z_gps_v)); - param_get(h->w_z_vision_p, &(p->w_z_vision_p)); - param_get(h->w_z_lidar, &(p->w_z_lidar)); - param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); - param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); - param_get(h->w_xy_vision_p, &(p->w_xy_vision_p)); - param_get(h->w_xy_vision_v, &(p->w_xy_vision_v)); - param_get(h->w_mocap_p, &(p->w_mocap_p)); - param_get(h->w_xy_flow, &(p->w_xy_flow)); - param_get(h->w_xy_res_v, &(p->w_xy_res_v)); - param_get(h->w_gps_flow, &(p->w_gps_flow)); - param_get(h->w_acc_bias, &(p->w_acc_bias)); - param_get(h->flow_k, &(p->flow_k)); - param_get(h->flow_q_min, &(p->flow_q_min)); - param_get(h->lidar_err, &(p->lidar_err)); - param_get(h->land_t, &(p->land_t)); - param_get(h->land_disp, &(p->land_disp)); - param_get(h->land_thr, &(p->land_thr)); - param_get(h->no_vision, &(p->no_vision)); - param_get(h->delay_gps, &(p->delay_gps)); - param_get(h->flow_module_offset_x, &(p->flow_module_offset_x)); - param_get(h->flow_module_offset_y, &(p->flow_module_offset_y)); - param_get(h->disable_mocap, &(p->disable_mocap)); - param_get(h->enable_lidar_alt_est, &(p->enable_lidar_alt_est)); - param_get(h->lidar_calibration_offset, &(p->lidar_calibration_offset)); - param_get(h->att_ext_hdg_m, &(p->att_ext_hdg_m)); - - return 0; -} diff --git a/src/examples/position_estimator_inav/position_estimator_inav_params.h b/src/examples/position_estimator_inav/position_estimator_inav_params.h deleted file mode 100644 index d8e6de7a16..0000000000 --- a/src/examples/position_estimator_inav/position_estimator_inav_params.h +++ /dev/null @@ -1,121 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013, 2014 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 position_estimator_inav_params.c - * - * @author Anton Babushkin - * - * Parameters definition for position_estimator_inav - */ - -#pragma once - -#include - -struct position_estimator_inav_params { - float w_z_baro; - float w_z_gps_p; - float w_z_gps_v; - float w_z_vision_p; - float w_z_lidar; - float w_xy_gps_p; - float w_xy_gps_v; - float w_xy_vision_p; - float w_xy_vision_v; - float w_mocap_p; - float w_xy_flow; - float w_xy_res_v; - float w_gps_flow; - float w_acc_bias; - float flow_k; - float flow_q_min; - float lidar_err; - float land_t; - float land_disp; - float land_thr; - int32_t no_vision; - float delay_gps; - float flow_module_offset_x; - float flow_module_offset_y; - int32_t disable_mocap; - int32_t enable_lidar_alt_est; - float lidar_calibration_offset; - int32_t att_ext_hdg_m; -}; - -struct position_estimator_inav_param_handles { - param_t w_z_baro; - param_t w_z_gps_p; - param_t w_z_gps_v; - param_t w_z_vision_p; - param_t w_z_lidar; - param_t w_xy_gps_p; - param_t w_xy_gps_v; - param_t w_xy_vision_p; - param_t w_xy_vision_v; - param_t w_mocap_p; - param_t w_xy_flow; - param_t w_xy_res_v; - param_t w_gps_flow; - param_t w_acc_bias; - param_t flow_k; - param_t flow_q_min; - param_t lidar_err; - param_t land_t; - param_t land_disp; - param_t land_thr; - param_t no_vision; - param_t delay_gps; - param_t flow_module_offset_x; - param_t flow_module_offset_y; - param_t disable_mocap; - param_t enable_lidar_alt_est; - param_t lidar_calibration_offset; - param_t att_ext_hdg_m; -}; - -#define CBRK_NO_VISION_KEY 328754 - -/** - * Initialize all parameter handles and values - * - */ -int inav_parameters_init(struct position_estimator_inav_param_handles *h); - -/** - * Update all parameters - * - */ -int inav_parameters_update(const struct position_estimator_inav_param_handles *h, - struct position_estimator_inav_params *p);