From f9794e99f897b842e74a370e5fa77c281aeddce5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 11 Mar 2020 21:20:54 -0400 Subject: [PATCH] move hover_thrust_estimator to new module (mc_hover_thrust_estimator) * MC_HTE: unitialize with hover_thrust parameter * MC_HTE: constrain hover thrust setter between 0.1 and 0.9 * MC_HTE: integrate with land detector and velocity controller * MCHoverThrustEstimator: Always publish an estimate even when not fusing measurements. This is required as the land detector and the position controller need to receive a hover thrust value. * MC_HTE: use altitude agl threshold to start the estimator local_position.z is relative to the origin of the EKF while dist_bottom is above ground Co-authored-by: bresch --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 1 + ROMFS/px4fmu_common/init.d/rc.vtol_apps | 1 + boards/aerotenna/ocpoc/default.cmake | 1 + boards/airmind/mindpx-v2/default.cmake | 1 + boards/atlflight/eagle/default.cmake | 1 + boards/atlflight/eagle/qurt.cmake | 1 + boards/atlflight/excelsior/default.cmake | 1 + boards/atlflight/excelsior/qurt.cmake | 1 + boards/av/x-v1/default.cmake | 1 + boards/beaglebone/blue/default.cmake | 1 + boards/bitcraze/crazyflie/default.cmake | 1 + boards/emlid/navio2/default.cmake | 1 + boards/holybro/durandal-v1/default.cmake | 1 + boards/holybro/durandal-v1/stackcheck.cmake | 1 + boards/holybro/kakutef7/default.cmake | 1 + boards/intel/aerofc-v1/default.cmake | 1 + boards/intel/aerofc-v1/rtps.cmake | 1 + boards/modalai/fc-v1/default.cmake | 1 + boards/mro/ctrl-zero-f7/default.cmake | 1 + boards/mro/x21-777/default.cmake | 1 + boards/mro/x21/default.cmake | 1 + boards/nxp/fmuk66-v3/default.cmake | 1 + boards/nxp/fmurt1062-v1/default.cmake | 1 + boards/omnibus/f4sd/default.cmake | 1 + boards/px4/fmu-v2/default.cmake | 1 + boards/px4/fmu-v2/multicopter.cmake | 1 + boards/px4/fmu-v2/test.cmake | 1 + boards/px4/fmu-v3/default.cmake | 1 + boards/px4/fmu-v3/rtps.cmake | 1 + boards/px4/fmu-v4/default.cmake | 1 + boards/px4/fmu-v4/rtps.cmake | 1 + boards/px4/fmu-v4/stackcheck.cmake | 1 + boards/px4/fmu-v4pro/default.cmake | 1 + boards/px4/fmu-v4pro/rtps.cmake | 1 + boards/px4/fmu-v5/critmonitor.cmake | 1 + boards/px4/fmu-v5/default.cmake | 1 + boards/px4/fmu-v5/irqmonitor.cmake | 1 + boards/px4/fmu-v5/multicopter.cmake | 1 + boards/px4/fmu-v5/rtps.cmake | 1 + boards/px4/fmu-v5/stackcheck.cmake | 1 + boards/px4/fmu-v5x/default.cmake | 1 + .../px4/fmu-v5x/p2_base_phy_LAN8742Ai.cmake | 1 + boards/px4/raspberrypi/default.cmake | 1 + boards/px4/sitl/default.cmake | 1 + boards/px4/sitl/rtps.cmake | 1 + boards/px4/sitl/test.cmake | 1 + boards/uvify/core/default.cmake | 1 + platforms/qurt/src/px4/common/commands_hil.c | 1 + posix-configs/SITL/init/test/test_shutdown | 1 + posix-configs/bbblue/px4.config | 1 + posix-configs/eagle/200qx/px4.config | 1 + posix-configs/eagle/210qc/px4.config | 1 + posix-configs/eagle/flight/px4.config | 1 + posix-configs/eagle/hil/px4.config | 1 + posix-configs/eagle/init/rcS | 1 + posix-configs/excelsior/px4.config | 1 + posix-configs/ocpoc/px4.config | 1 + posix-configs/rpi/px4.config | 1 + posix-configs/rpi/px4_hil.config | 1 + posix-configs/rpi/px4_test.config | 1 + src/lib/CMakeLists.txt | 1 - .../hover_thrust_estimator.cpp | 86 ------- .../land_detector/MulticopterLandDetector.cpp | 18 +- .../land_detector/MulticopterLandDetector.h | 4 + .../mc_hover_thrust_estimator}/CMakeLists.txt | 18 +- .../MulticopterHoverThrustEstimator.cpp | 235 ++++++++++++++++++ .../MulticopterHoverThrustEstimator.hpp} | 74 ++++-- .../hover_thrust_estimator_params.c | 0 .../zero_order_hover_thrust_ekf.cpp | 2 - .../zero_order_hover_thrust_ekf.hpp | 5 +- .../zero_order_hover_thrust_ekf_test.cpp | 0 src/modules/mc_pos_control/CMakeLists.txt | 1 - .../mc_pos_control/mc_pos_control_main.cpp | 44 ++-- .../mc_pos_control/mc_pos_control_params.c | 12 + 74 files changed, 419 insertions(+), 141 deletions(-) delete mode 100644 src/lib/hover_thrust_estimator/hover_thrust_estimator.cpp rename src/{lib/hover_thrust_estimator => modules/mc_hover_thrust_estimator}/CMakeLists.txt (83%) create mode 100644 src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp rename src/{lib/hover_thrust_estimator/hover_thrust_estimator.hpp => modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp} (60%) rename src/{lib/hover_thrust_estimator => modules/mc_hover_thrust_estimator}/hover_thrust_estimator_params.c (100%) rename src/{lib/hover_thrust_estimator => modules/mc_hover_thrust_estimator}/zero_order_hover_thrust_ekf.cpp (99%) rename src/{lib/hover_thrust_estimator => modules/mc_hover_thrust_estimator}/zero_order_hover_thrust_ekf.hpp (95%) rename src/{lib/hover_thrust_estimator => modules/mc_hover_thrust_estimator}/zero_order_hover_thrust_ekf_test.cpp (100%) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index e26050bed1..32f2f18df0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -61,6 +61,7 @@ mc_att_control start # # Start Multicopter Position Controller. # +mc_hover_thrust_estimator start mc_pos_control start # diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index c5ee5b2e20..40a1e3df23 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -22,6 +22,7 @@ vtol_att_control start mc_rate_control start vtol mc_att_control start vtol mc_pos_control start vtol +mc_hover_thrust_estimator start fw_att_control start vtol fw_pos_control_l1 start vtol diff --git a/boards/aerotenna/ocpoc/default.cmake b/boards/aerotenna/ocpoc/default.cmake index 95dbbe203c..b5a8a1c4fe 100644 --- a/boards/aerotenna/ocpoc/default.cmake +++ b/boards/aerotenna/ocpoc/default.cmake @@ -51,6 +51,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 5e224793f1..c8c8fbb085 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -69,6 +69,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control navigator diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index 10447de029..a20c6ba605 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -72,6 +72,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/atlflight/eagle/qurt.cmake b/boards/atlflight/eagle/qurt.cmake index 141e5a5825..81a0a0166b 100644 --- a/boards/atlflight/eagle/qurt.cmake +++ b/boards/atlflight/eagle/qurt.cmake @@ -62,6 +62,7 @@ px4_add_board( landing_target_estimator local_position_estimator mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control muorb/adsp diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index dc03cd2675..0867448924 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -72,6 +72,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/atlflight/excelsior/qurt.cmake b/boards/atlflight/excelsior/qurt.cmake index 539a32a37f..07f8c75d04 100644 --- a/boards/atlflight/excelsior/qurt.cmake +++ b/boards/atlflight/excelsior/qurt.cmake @@ -62,6 +62,7 @@ px4_add_board( landing_target_estimator local_position_estimator mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control muorb/adsp diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 763cce00df..ceb753274b 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -69,6 +69,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control navigator diff --git a/boards/beaglebone/blue/default.cmake b/boards/beaglebone/blue/default.cmake index 1a427fcabe..712bf3264e 100644 --- a/boards/beaglebone/blue/default.cmake +++ b/boards/beaglebone/blue/default.cmake @@ -43,6 +43,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake index 523ff3b75d..c3eeca6398 100644 --- a/boards/bitcraze/crazyflie/default.cmake +++ b/boards/bitcraze/crazyflie/default.cmake @@ -27,6 +27,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control navigator diff --git a/boards/emlid/navio2/default.cmake b/boards/emlid/navio2/default.cmake index a5e503953b..ff7ce0af1c 100644 --- a/boards/emlid/navio2/default.cmake +++ b/boards/emlid/navio2/default.cmake @@ -45,6 +45,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/holybro/durandal-v1/default.cmake b/boards/holybro/durandal-v1/default.cmake index 50b0cc9d81..60ab65ff0b 100644 --- a/boards/holybro/durandal-v1/default.cmake +++ b/boards/holybro/durandal-v1/default.cmake @@ -74,6 +74,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control navigator diff --git a/boards/holybro/durandal-v1/stackcheck.cmake b/boards/holybro/durandal-v1/stackcheck.cmake index 55268c5dbc..d9634fe66c 100644 --- a/boards/holybro/durandal-v1/stackcheck.cmake +++ b/boards/holybro/durandal-v1/stackcheck.cmake @@ -75,6 +75,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control navigator diff --git a/boards/holybro/kakutef7/default.cmake b/boards/holybro/kakutef7/default.cmake index 8c8aa301da..b3db1fd2e7 100644 --- a/boards/holybro/kakutef7/default.cmake +++ b/boards/holybro/kakutef7/default.cmake @@ -41,6 +41,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control navigator diff --git a/boards/intel/aerofc-v1/default.cmake b/boards/intel/aerofc-v1/default.cmake index 5fde7ef38f..3f65e3140f 100644 --- a/boards/intel/aerofc-v1/default.cmake +++ b/boards/intel/aerofc-v1/default.cmake @@ -48,6 +48,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/intel/aerofc-v1/rtps.cmake b/boards/intel/aerofc-v1/rtps.cmake index 51f03b652d..e071118aa3 100644 --- a/boards/intel/aerofc-v1/rtps.cmake +++ b/boards/intel/aerofc-v1/rtps.cmake @@ -48,6 +48,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control micrortps_bridge diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index d3ea79a517..c0401fe5cb 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -67,6 +67,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control navigator diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index b1f06f08b7..c4f7aeefeb 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -72,6 +72,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/mro/x21-777/default.cmake b/boards/mro/x21-777/default.cmake index 3cb79f35ed..7f34d7dd32 100644 --- a/boards/mro/x21-777/default.cmake +++ b/boards/mro/x21-777/default.cmake @@ -66,6 +66,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/mro/x21/default.cmake b/boards/mro/x21/default.cmake index abd9ae4318..0ae2dcbe69 100644 --- a/boards/mro/x21/default.cmake +++ b/boards/mro/x21/default.cmake @@ -69,6 +69,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index 75d658cfbd..0f1a57ca73 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -71,6 +71,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control navigator diff --git a/boards/nxp/fmurt1062-v1/default.cmake b/boards/nxp/fmurt1062-v1/default.cmake index 16abac6690..12d5a6ad50 100644 --- a/boards/nxp/fmurt1062-v1/default.cmake +++ b/boards/nxp/fmurt1062-v1/default.cmake @@ -63,6 +63,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control navigator diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake index f1ef9de8b8..44dbbf169e 100644 --- a/boards/omnibus/f4sd/default.cmake +++ b/boards/omnibus/f4sd/default.cmake @@ -59,6 +59,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control navigator diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index b092eb947c..df96602b08 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -81,6 +81,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control navigator diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake index d97b61914c..002f086ad3 100644 --- a/boards/px4/fmu-v2/multicopter.cmake +++ b/boards/px4/fmu-v2/multicopter.cmake @@ -49,6 +49,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control navigator diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake index 8b5ef9163a..ebe1510e6b 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -79,6 +79,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control navigator diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index c68390e7e6..b598b073b7 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -78,6 +78,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index 021aec6e1b..9c5b853b5a 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -78,6 +78,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control micrortps_bridge diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 9eb27da837..e452147cf3 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -72,6 +72,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index 5ad397bc19..67dca406b0 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -72,6 +72,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control micrortps_bridge diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index 4fc59edcb8..a342f33e4b 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -72,6 +72,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 5023dfb1aa..63b3644d0d 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -74,6 +74,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index c47c313a50..4b18bf554e 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -74,6 +74,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control micrortps_bridge diff --git a/boards/px4/fmu-v5/critmonitor.cmake b/boards/px4/fmu-v5/critmonitor.cmake index f2c1bc0546..4ce1743d47 100644 --- a/boards/px4/fmu-v5/critmonitor.cmake +++ b/boards/px4/fmu-v5/critmonitor.cmake @@ -74,6 +74,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 2bbdd08e28..fc49507669 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -77,6 +77,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/px4/fmu-v5/irqmonitor.cmake b/boards/px4/fmu-v5/irqmonitor.cmake index dab94bb435..986a875505 100644 --- a/boards/px4/fmu-v5/irqmonitor.cmake +++ b/boards/px4/fmu-v5/irqmonitor.cmake @@ -74,6 +74,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index 7eb6b0a9bd..1f9febb998 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -63,6 +63,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control navigator diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index 261a57720a..db21de11b3 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -74,6 +74,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control micrortps_bridge diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index b8d24b3cc1..c78a7fe4ab 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -75,6 +75,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index 8ef136e073..1ce6bb2c59 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -75,6 +75,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/px4/fmu-v5x/p2_base_phy_LAN8742Ai.cmake b/boards/px4/fmu-v5x/p2_base_phy_LAN8742Ai.cmake index f3a8c85e72..25ce33cc0e 100644 --- a/boards/px4/fmu-v5x/p2_base_phy_LAN8742Ai.cmake +++ b/boards/px4/fmu-v5x/p2_base_phy_LAN8742Ai.cmake @@ -75,6 +75,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/px4/raspberrypi/default.cmake b/boards/px4/raspberrypi/default.cmake index ad42959334..c71370a53b 100644 --- a/boards/px4/raspberrypi/default.cmake +++ b/boards/px4/raspberrypi/default.cmake @@ -44,6 +44,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control #micrortps_bridge diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 71000c06c5..d93074c592 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -36,6 +36,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control navigator diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index 84dc097359..8af7c7fba6 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -37,6 +37,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control micrortps_bridge diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index e8bac51580..ab36932b58 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -36,6 +36,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control navigator diff --git a/boards/uvify/core/default.cmake b/boards/uvify/core/default.cmake index 2a2fc1c029..279eaa83c8 100644 --- a/boards/uvify/core/default.cmake +++ b/boards/uvify/core/default.cmake @@ -51,6 +51,7 @@ px4_add_board( logger mavlink mc_att_control + mc_hover_thrust_estimator mc_pos_control mc_rate_control navigator diff --git a/platforms/qurt/src/px4/common/commands_hil.c b/platforms/qurt/src/px4/common/commands_hil.c index 4a5a4f22a7..acfa8a971b 100644 --- a/platforms/qurt/src/px4/common/commands_hil.c +++ b/platforms/qurt/src/px4/common/commands_hil.c @@ -53,6 +53,7 @@ const char *get_commands() "commander start --hil\n" "sensors start\n" "ekf2 start\n" + "mc_hover_thrust_estimator start\n" "mc_pos_control start\n" "mc_att_control start\n" "mc_rate_control start\n" diff --git a/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown index ccaead269f..684813a462 100644 --- a/posix-configs/SITL/init/test/test_shutdown +++ b/posix-configs/SITL/init/test/test_shutdown @@ -25,6 +25,7 @@ land_detector start vtol navigator start ekf2 start vtol_att_control start +mc_hover_thrust_estimator start mc_pos_control start vtol mc_att_control start vtol mc_rate_control start vtol diff --git a/posix-configs/bbblue/px4.config b/posix-configs/bbblue/px4.config index 0d205ac7c8..a7d6883398 100644 --- a/posix-configs/bbblue/px4.config +++ b/posix-configs/bbblue/px4.config @@ -67,6 +67,7 @@ navigator start ekf2 start land_detector start multicopter +mc_hover_thrust_estimator start mc_pos_control start mc_att_control start mc_rate_control start diff --git a/posix-configs/eagle/200qx/px4.config b/posix-configs/eagle/200qx/px4.config index 9eb017d110..3578a9d286 100644 --- a/posix-configs/eagle/200qx/px4.config +++ b/posix-configs/eagle/200qx/px4.config @@ -26,6 +26,7 @@ sensors start commander start ekf2 start land_detector start multicopter +mc_hover_thrust_estimator start mc_pos_control start mc_att_control start mc_rate_control start diff --git a/posix-configs/eagle/210qc/px4.config b/posix-configs/eagle/210qc/px4.config index 36c69c9149..91a77144c0 100644 --- a/posix-configs/eagle/210qc/px4.config +++ b/posix-configs/eagle/210qc/px4.config @@ -26,6 +26,7 @@ sensors start commander start ekf2 start land_detector start multicopter +mc_hover_thrust_estimator start mc_pos_control start mc_att_control start mc_rate_control start diff --git a/posix-configs/eagle/flight/px4.config b/posix-configs/eagle/flight/px4.config index 3bb8c9fa75..75a91acbd2 100644 --- a/posix-configs/eagle/flight/px4.config +++ b/posix-configs/eagle/flight/px4.config @@ -14,6 +14,7 @@ sensors start commander start ekf2 start land_detector start multicopter +mc_hover_thrust_estimator start mc_pos_control start mc_att_control start mc_rate_control start diff --git a/posix-configs/eagle/hil/px4.config b/posix-configs/eagle/hil/px4.config index 539ff4350d..8670e19066 100644 --- a/posix-configs/eagle/hil/px4.config +++ b/posix-configs/eagle/hil/px4.config @@ -16,6 +16,7 @@ rc_update start commander start -hil sensors start ekf2 start +mc_hover_thrust_estimator start mc_pos_control start mc_att_control start mc_rate_control start diff --git a/posix-configs/eagle/init/rcS b/posix-configs/eagle/init/rcS index 4018151392..10bde2c430 100644 --- a/posix-configs/eagle/init/rcS +++ b/posix-configs/eagle/init/rcS @@ -87,6 +87,7 @@ fi qshell commander start qshell land_detector start multicopter +qshell mc_hover_thrust_estimator start qshell mc_pos_control start qshell mc_att_control start qshell mc_rate_control start diff --git a/posix-configs/excelsior/px4.config b/posix-configs/excelsior/px4.config index 45f7f0866e..eaaf5d0314 100644 --- a/posix-configs/excelsior/px4.config +++ b/posix-configs/excelsior/px4.config @@ -24,6 +24,7 @@ sensors start commander start ekf2 start land_detector start multicopter +mc_hover_thrust_estimator start mc_pos_control start mc_att_control start mc_rate_control start diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config index 623ab1f81a..0e6d4ce00c 100644 --- a/posix-configs/ocpoc/px4.config +++ b/posix-configs/ocpoc/px4.config @@ -32,6 +32,7 @@ navigator start dataman start ekf2 start land_detector start multicopter +mc_hover_thrust_estimator start mc_pos_control start mc_att_control start mc_rate_control start diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index 1894ced585..9092efd963 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -36,6 +36,7 @@ commander start navigator start ekf2 start land_detector start multicopter +mc_hover_thrust_estimator start mc_pos_control start mc_att_control start mc_rate_control start diff --git a/posix-configs/rpi/px4_hil.config b/posix-configs/rpi/px4_hil.config index 216cac86e8..515d88dbdd 100644 --- a/posix-configs/rpi/px4_hil.config +++ b/posix-configs/rpi/px4_hil.config @@ -25,6 +25,7 @@ commander start -hil navigator start ekf2 start land_detector start multicopter +mc_hover_thrust_estimator start mc_pos_control start mc_att_control start mc_rate_control start diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index f552b3610d..06a0b81333 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -36,6 +36,7 @@ commander start navigator start ekf2 start land_detector start multicopter +mc_hover_thrust_estimator start mc_pos_control start mc_att_control start diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 09f9ebffbb..e4321183b2 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -47,7 +47,6 @@ add_subdirectory(conversion) add_subdirectory(drivers) add_subdirectory(ecl) add_subdirectory(flight_tasks) -add_subdirectory(hover_thrust_estimator) add_subdirectory(hysteresis) add_subdirectory(landing_slope) add_subdirectory(led) diff --git a/src/lib/hover_thrust_estimator/hover_thrust_estimator.cpp b/src/lib/hover_thrust_estimator/hover_thrust_estimator.cpp deleted file mode 100644 index 253e5ed6f8..0000000000 --- a/src/lib/hover_thrust_estimator/hover_thrust_estimator.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 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 hover_thrust_estimator.cpp - * - * @author Mathieu Bresciani - */ - -#include "hover_thrust_estimator.hpp" - -void HoverThrustEstimator::reset() -{ - _thrust = 0.f; - _acc_z = 0.f; - _hover_thrust_ekf.setHoverThrustStdDev(_param_hte_ht_err_init.get()); - _hover_thrust_ekf.resetAccelNoise(); - -} - -void HoverThrustEstimator::updateParams() -{ - const float ht_err_init_prev = _param_hte_ht_err_init.get(); - ModuleParams::updateParams(); - - _hover_thrust_ekf.setProcessNoiseStdDev(_param_hte_ht_noise.get()); - - if (fabsf(_param_hte_ht_err_init.get() - ht_err_init_prev) > FLT_EPSILON) { - _hover_thrust_ekf.setHoverThrustStdDev(_param_hte_ht_err_init.get()); - } - - _hover_thrust_ekf.setAccelInnovGate(_param_hte_acc_gate.get()); -} - -void HoverThrustEstimator::update(const float dt) -{ - _hover_thrust_ekf.predict(dt); - - ZeroOrderHoverThrustEkf::status status{}; - _hover_thrust_ekf.fuseAccZ(_acc_z, _thrust, status); - - publishStatus(status); -} - -void HoverThrustEstimator::publishStatus(ZeroOrderHoverThrustEkf::status &status) -{ - hover_thrust_estimate_s status_msg{}; - status_msg.timestamp = hrt_absolute_time(); - status_msg.hover_thrust = status.hover_thrust; - status_msg.hover_thrust_var = status.hover_thrust_var; - status_msg.accel_innov = status.innov; - status_msg.accel_innov_var = status.innov_var; - status_msg.accel_innov_test_ratio = status.innov_test_ratio; - status_msg.accel_noise_var = status.accel_noise_var; - _hover_thrust_ekf_pub.publish(status_msg); -} diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 79176247b1..7541768252 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -76,6 +76,7 @@ MulticopterLandDetector::MulticopterLandDetector() _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); _paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN"); _paramHandle.minThrottle = param_find("MPC_THR_MIN"); + _paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE"); _paramHandle.hoverThrottle = param_find("MPC_THR_HOVER"); // Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true). @@ -93,6 +94,14 @@ void MulticopterLandDetector::_update_topics() _vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity); _vehicle_control_mode_sub.update(&_vehicle_control_mode); _vehicle_local_position_setpoint_sub.update(&_vehicle_local_position_setpoint); + + if (_params.useHoverThrustEstimate) { + hover_thrust_estimate_s hte; + + if (_hover_thrust_estimate_sub.update(&hte)) { + _params.hoverThrottle = hte.hover_thrust; + } + } } void MulticopterLandDetector::_update_params() @@ -102,9 +111,16 @@ void MulticopterLandDetector::_update_params() _freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _param_lndmc_ffall_ttri.get())); param_get(_paramHandle.minThrottle, &_params.minThrottle); - param_get(_paramHandle.hoverThrottle, &_params.hoverThrottle); param_get(_paramHandle.minManThrottle, &_params.minManThrottle); param_get(_paramHandle.landSpeed, &_params.landSpeed); + + int32_t use_hover_thrust_estimate = 0; + param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate); + _params.useHoverThrustEstimate = (use_hover_thrust_estimate == 1); + + if (!_params.useHoverThrustEstimate) { + param_get(_paramHandle.hoverThrottle, &_params.hoverThrottle); + } } bool MulticopterLandDetector::_get_freefall_state() diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index c1ea3d2e9a..a9d6abb650 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -48,6 +48,7 @@ #include #include #include +#include #include "LandDetector.h" @@ -102,6 +103,7 @@ private: param_t hoverThrottle; param_t minManThrottle; param_t landSpeed; + param_t useHoverThrustEstimate; } _paramHandle{}; struct { @@ -109,6 +111,7 @@ private: float hoverThrottle; float minManThrottle; float landSpeed; + bool useHoverThrustEstimate; } _params{}; uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)}; @@ -118,6 +121,7 @@ private: uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; actuator_controls_s _actuator_controls {}; battery_status_s _battery_status {}; diff --git a/src/lib/hover_thrust_estimator/CMakeLists.txt b/src/modules/mc_hover_thrust_estimator/CMakeLists.txt similarity index 83% rename from src/lib/hover_thrust_estimator/CMakeLists.txt rename to src/modules/mc_hover_thrust_estimator/CMakeLists.txt index 1088b05875..d924da3bb1 100644 --- a/src/lib/hover_thrust_estimator/CMakeLists.txt +++ b/src/modules/mc_hover_thrust_estimator/CMakeLists.txt @@ -31,9 +31,21 @@ # ############################################################################ -px4_add_library(HoverThrustEstimator - hover_thrust_estimator.cpp +px4_add_library(zero_order_hover_thrust_ekf zero_order_hover_thrust_ekf.cpp + zero_order_hover_thrust_ekf.hpp ) -px4_add_unit_gtest(SRC zero_order_hover_thrust_ekf_test.cpp LINKLIBS HoverThrustEstimator) +px4_add_module( + MODULE modules__mc_hover_thrust_estimator + MAIN mc_hover_thrust_estimator + SRCS + MulticopterHoverThrustEstimator.cpp + MulticopterHoverThrustEstimator.hpp + DEPENDS + mathlib + px4_work_queue + zero_order_hover_thrust_ekf +) + +px4_add_unit_gtest(SRC zero_order_hover_thrust_ekf_test.cpp LINKLIBS zero_order_hover_thrust_ekf) diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp new file mode 100644 index 0000000000..57ca54b9a3 --- /dev/null +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -0,0 +1,235 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 hover_thrust_estimator.cpp + * + * @author Mathieu Bresciani + */ + +#include "MulticopterHoverThrustEstimator.hpp" + +#include + +MulticopterHoverThrustEstimator::MulticopterHoverThrustEstimator() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ + updateParams(); + reset(); +} + +MulticopterHoverThrustEstimator::~MulticopterHoverThrustEstimator() +{ + perf_free(_cycle_perf); +} + +bool MulticopterHoverThrustEstimator::init() +{ + if (!_vehicle_local_position_setpoint_sub.registerCallback()) { + PX4_ERR("vehicle_local_position_setpoint callback registration failed!"); + return false; + } + + return true; +} + +void MulticopterHoverThrustEstimator::reset() +{ + _hover_thrust_ekf.setHoverThrust(_param_mpc_thr_hover.get()); + _hover_thrust_ekf.setHoverThrustStdDev(_param_hte_ht_err_init.get()); + _hover_thrust_ekf.resetAccelNoise(); +} + +void MulticopterHoverThrustEstimator::updateParams() +{ + const float ht_err_init_prev = _param_hte_ht_err_init.get(); + ModuleParams::updateParams(); + + _hover_thrust_ekf.setProcessNoiseStdDev(_param_hte_ht_noise.get()); + + if (fabsf(_param_hte_ht_err_init.get() - ht_err_init_prev) > FLT_EPSILON) { + _hover_thrust_ekf.setHoverThrustStdDev(_param_hte_ht_err_init.get()); + } + + _hover_thrust_ekf.setAccelInnovGate(_param_hte_acc_gate.get()); +} + +void MulticopterHoverThrustEstimator::Run() +{ + if (should_exit()) { + _vehicle_local_position_setpoint_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + } + + perf_begin(_cycle_perf); + + vehicle_land_detected_s vehicle_land_detected; + vehicle_status_s vehicle_status; + vehicle_local_position_s local_pos; + + if (_vehicle_land_detected_sub.update(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + } + + if (_vehicle_status_sub.update(&vehicle_status)) { + _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + } + + if (_vehicle_local_pos_sub.update(&local_pos)) { + // This is only necessary because the landed + // flag of the land detector does not guarantee that + // the vehicle does not touch the ground anymore + // TODO: improve the landed flag + _in_air = local_pos.dist_bottom > 1.f; + } + + ZeroOrderHoverThrustEkf::status status{}; + + if (_armed && !_landed && _in_air) { + vehicle_local_position_setpoint_s local_pos_sp; + + if (_vehicle_local_position_setpoint_sub.update(&local_pos_sp)) { + + const hrt_abstime now = hrt_absolute_time(); + const float dt = math::constrain((now - _timestamp_last) / 1e6f, 0.002f, 0.2f); + + _hover_thrust_ekf.predict(dt); + + if (PX4_ISFINITE(local_pos.az) && PX4_ISFINITE(local_pos_sp.thrust[2])) { + // Inform the hover thrust estimator about the measured vertical + // acceleration (positive acceleration is up) and the current thrust (positive thrust is up) + _hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2], status); + } + } + + } else { + if (!_armed) { + reset(); + } + + status.hover_thrust = _hover_thrust_ekf.getHoverThrustEstimate(); + status.hover_thrust_var = _hover_thrust_ekf.getHoverThrustEstimateVar(); + status.accel_noise_var = _hover_thrust_ekf.getAccelNoiseVar(); + status.innov = NAN; + status.innov_var = NAN; + status.innov_test_ratio = NAN; + } + + publishStatus(status); + + perf_end(_cycle_perf); +} + +void MulticopterHoverThrustEstimator::publishStatus(ZeroOrderHoverThrustEkf::status &status) +{ + hover_thrust_estimate_s status_msg{}; + status_msg.hover_thrust = status.hover_thrust; + status_msg.hover_thrust_var = status.hover_thrust_var; + status_msg.accel_innov = status.innov; + status_msg.accel_innov_var = status.innov_var; + status_msg.accel_innov_test_ratio = status.innov_test_ratio; + status_msg.accel_noise_var = status.accel_noise_var; + status_msg.timestamp = hrt_absolute_time(); + _hover_thrust_ekf_pub.publish(status_msg); +} + +int MulticopterHoverThrustEstimator::task_spawn(int argc, char *argv[]) +{ + MulticopterHoverThrustEstimator *instance = new MulticopterHoverThrustEstimator(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int MulticopterHoverThrustEstimator::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int MulticopterHoverThrustEstimator::print_status() +{ + perf_print_counter(_cycle_perf); + + return 0; +} + +int MulticopterHoverThrustEstimator::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("mc_hover_thrust_estimator", "estimator"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int mc_hover_thrust_estimator_main(int argc, char *argv[]) +{ + return MulticopterHoverThrustEstimator::main(argc, argv); +} diff --git a/src/lib/hover_thrust_estimator/hover_thrust_estimator.hpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp similarity index 60% rename from src/lib/hover_thrust_estimator/hover_thrust_estimator.hpp rename to src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp index 2c4aaf6519..5e16c997a2 100644 --- a/src/lib/hover_thrust_estimator/hover_thrust_estimator.hpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp @@ -41,48 +41,76 @@ #pragma once -#include -#include -#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "zero_order_hover_thrust_ekf.hpp" -class HoverThrustEstimator : public ModuleParams +class MulticopterHoverThrustEstimator : public ModuleBase, public ModuleParams, + public px4::WorkItem { public: - HoverThrustEstimator(ModuleParams *parent) : - ModuleParams(parent) - { - ZeroOrderHoverThrustEkf::status status{}; - publishStatus(status); - } - ~HoverThrustEstimator() = default; + MulticopterHoverThrustEstimator(); + ~MulticopterHoverThrustEstimator() override; - void reset(); + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); - void update(float dt); + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); - void setThrust(float thrust) { _thrust = thrust; }; - void setAccel(float accel) { _acc_z = accel; }; + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); - float getHoverThrustEstimate() const { return _hover_thrust_ekf.getHoverThrustEstimate(); } + bool init(); -protected: - void updateParams() override; + /** @see ModuleBase::print_status() */ + int print_status() override; private: + void Run() override; + void updateParams() override; + + void reset(); void publishStatus(ZeroOrderHoverThrustEkf::status &status); ZeroOrderHoverThrustEkf _hover_thrust_ekf{}; - float _acc_z{}; - float _thrust{}; + + uORB::Publication _hover_thrust_ekf_pub{ORB_ID(hover_thrust_estimate)}; + + uORB::SubscriptionCallbackWorkItem _vehicle_local_position_setpoint_sub{this, ORB_ID(vehicle_local_position_setpoint)}; + + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_local_pos_sub{ORB_ID(vehicle_local_position)}; + + hrt_abstime _timestamp_last{0}; + + bool _armed{false}; + bool _landed{false}; + bool _in_air{false}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; DEFINE_PARAMETERS( (ParamFloat) _param_hte_ht_noise, (ParamFloat) _param_hte_acc_gate, - (ParamFloat) _param_hte_ht_err_init + (ParamFloat) _param_hte_ht_err_init, + (ParamFloat) _param_mpc_thr_hover ) - - uORB::Publication _hover_thrust_ekf_pub{ORB_ID(hover_thrust_estimate)}; }; diff --git a/src/lib/hover_thrust_estimator/hover_thrust_estimator_params.c b/src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c similarity index 100% rename from src/lib/hover_thrust_estimator/hover_thrust_estimator_params.c rename to src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c diff --git a/src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp similarity index 99% rename from src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp rename to src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp index 69e669f901..28227dcec5 100644 --- a/src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp +++ b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp @@ -37,8 +37,6 @@ * @author Mathieu Bresciani */ -#include - #include "zero_order_hover_thrust_ekf.hpp" void ZeroOrderHoverThrustEkf::predict(const float dt) diff --git a/src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp similarity index 95% rename from src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp rename to src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp index 4a63edad10..1bbe1f7596 100644 --- a/src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp +++ b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp @@ -65,6 +65,7 @@ #include #include +#include class ZeroOrderHoverThrustEkf { @@ -86,13 +87,15 @@ public: void predict(float _dt); void fuseAccZ(float acc_z, float thrust, status &status_return); - void setHoverThrust(float hover_thrust) { _hover_thr = hover_thrust; } + void setHoverThrust(float hover_thrust) { _hover_thr = math::constrain(hover_thrust, 0.1f, 0.9f); } void setProcessNoiseStdDev(float process_noise) { _process_var = process_noise * process_noise; } void setMeasurementNoiseStdDev(float measurement_noise) { _acc_var = measurement_noise * measurement_noise; } void setHoverThrustStdDev(float hover_thrust_noise) { _state_var = hover_thrust_noise * hover_thrust_noise; } void setAccelInnovGate(float gate_size) { _gate_size = gate_size; } float getHoverThrustEstimate() const { return _hover_thr; } + float getHoverThrustEstimateVar() const { return _state_var; } + float getAccelNoiseVar() const { return _acc_var; } private: float _hover_thr{0.5f}; diff --git a/src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp similarity index 100% rename from src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp rename to src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index 09872cffa3..a6394aced8 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -50,5 +50,4 @@ px4_add_module( WeatherVane CollisionPrevention Takeoff - HoverThrustEstimator ) 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 9f7bcd7752..add3c572a6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -45,7 +45,6 @@ #include #include #include -#include #include #include #include @@ -67,6 +66,7 @@ #include #include #include +#include #include "PositionControl/PositionControl.hpp" #include "Takeoff/Takeoff.hpp" @@ -122,6 +122,7 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; /**< vehicle attitude */ uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position */ + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ @@ -159,6 +160,7 @@ private: (ParamFloat) _param_mpc_z_vel_max_dn, (ParamFloat) _param_mpc_tiltmax_air, (ParamFloat) _param_mpc_thr_hover, + (ParamBool) _param_mpc_use_hte, // Takeoff / Land (ParamFloat) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */ @@ -184,8 +186,6 @@ private: PositionControl _control; /**< class for core PID position control */ PositionControlStates _states{}; /**< structure containing vehicle state information for position control */ - HoverThrustEstimator _hover_thrust_estimator; - hrt_abstime _last_warn = 0; /**< timer when the last warn message was sent out */ bool _in_failsafe = false; /**< true if failsafe was entered within current cycle */ @@ -283,7 +283,6 @@ MulticopterPositionControl::MulticopterPositionControl(bool vtol) : _vel_x_deriv(this, "VELD"), _vel_y_deriv(this, "VELD"), _vel_z_deriv(this, "VELD"), - _hover_thrust_estimator(this), _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")) { if (vtol) { @@ -351,7 +350,6 @@ MulticopterPositionControl::parameters_update(bool force) _control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get()); _control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get()); _control.setTiltLimit(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()); // convert to radians! - _control.setHoverThrust(_param_mpc_thr_hover.get()); // Check that the design parameters are inside the absolute maximum constraints if (_param_mpc_xy_cruise.get() > _param_mpc_xy_vel_max.get()) { @@ -366,12 +364,16 @@ MulticopterPositionControl::parameters_update(bool force) mavlink_log_critical(&_mavlink_log_pub, "Manual speed has been constrained by max speed"); } - if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() || - _param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) { - _param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(), - _param_mpc_thr_max.get())); - _param_mpc_thr_hover.commit(); - mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max"); + if (!_param_mpc_use_hte.get()) { + if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() || + _param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) { + _param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(), + _param_mpc_thr_max.get())); + _param_mpc_thr_hover.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max"); + } + + _control.setHoverThrust(_param_mpc_thr_hover.get()); } _flight_tasks.handleParameterUpdate(); @@ -408,6 +410,14 @@ MulticopterPositionControl::poll_subscriptions() _states.yaw = Eulerf(Quatf(att.q)).psi(); } } + + if (_param_mpc_use_hte.get()) { + hover_thrust_estimate_s hte; + + if (_hover_thrust_estimate_sub.update(&hte)) { + _control.setHoverThrust(hte.hover_thrust); + } + } } void @@ -594,15 +604,6 @@ MulticopterPositionControl::Run() _control.resetIntegral(); // reactivate the task which will reset the setpoint to current state _flight_tasks.reActivate(); - _hover_thrust_estimator.reset(); - - } else if (_takeoff.getTakeoffState() >= TakeoffState::flight) { - // Inform the hover thrust estimator about the measured vertical acceleration (positive acceleration is up) - if (PX4_ISFINITE(_local_pos.az)) { - _hover_thrust_estimator.setAccel(-_local_pos.az); - } - - _hover_thrust_estimator.update(_dt); } if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) { @@ -649,9 +650,6 @@ MulticopterPositionControl::Run() _flight_tasks.updateVelocityControllerIO(Vector3f(local_pos_sp.vx, local_pos_sp.vy, local_pos_sp.vz), Vector3f(local_pos_sp.thrust)); - // Inform the hover thrust estimator about the current thrust (positive thrust is up) - _hover_thrust_estimator.setThrust(-local_pos_sp.thrust[2]); - vehicle_attitude_setpoint_s attitude_setpoint{}; attitude_setpoint.timestamp = time_stamp_now; _control.getAttitudeSetpoint(attitude_setpoint); 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 4f9e153d45..b9e4bb9426 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -73,6 +73,18 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); */ PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f); +/** + * Hover thrust source selector + * + * Set false to use the fixed parameter MPC_THR_HOVER + * (EXPERIMENTAL) Set true to use the value computed by the hover thrust estimator + * + * @boolean + * @category Developer + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_USE_HTE, 0); + /** * Thrust curve in Manual Mode *