From cedf14e2ba453f0eee12da0dcf75bd2fa23cb049 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 7 Aug 2019 13:06:00 +0200 Subject: [PATCH] Airspeed Selector: repurpose wind estimator into an airspeed (selection, validation) module. This new airspeed module does: -runns an airspeed validator for every airspeed sensor present, which checks measurement validity and estimates an airspeed scale -selects another airspeed sensor if for the current one a failure is detected -estimates airspeed with groundspeed-windspeed if no valid airspeed sensor is present -outputs airspeed_validated topic Signed-off-by: Silvan Fuhrer --- ROMFS/px4fmu_common/init.d-posix/1030_plane | 1 - .../init.d-posix/1040_standard_vtol | 1 - .../init.d-posix/1041_tailsitter | 1 - .../px4fmu_common/init.d-posix/1042_tiltrotor | 1 - ROMFS/px4fmu_common/init.d-posix/rcS | 5 - ROMFS/px4fmu_common/init.d/rc.fw_apps | 3 +- ROMFS/px4fmu_common/init.d/rc.vtol_apps | 2 +- ROMFS/px4fmu_common/init.d/rcS | 8 - boards/aerotenna/ocpoc/ubuntu.cmake | 2 +- boards/airmind/mindpx-v2/default.cmake | 2 +- boards/atlflight/eagle/default.cmake | 2 +- boards/atlflight/eagle/qurt-default.cmake | 2 +- boards/atlflight/excelsior/default.cmake | 2 +- boards/atlflight/excelsior/qurt-default.cmake | 2 +- boards/auav/x21/default.cmake | 2 +- boards/av/x-v1/default.cmake | 2 +- boards/beaglebone/blue/cross.cmake | 2 +- boards/beaglebone/blue/native.cmake | 2 +- boards/bitcraze/crazyflie/default.cmake | 2 +- boards/emlid/navio2/cross.cmake | 2 +- boards/emlid/navio2/native.cmake | 2 +- boards/intel/aerofc-v1/default.cmake | 2 +- boards/intel/aerofc-v1/rtps.cmake | 2 +- boards/nxp/fmuk66-v3/default.cmake | 2 +- boards/omnibus/f4sd/default.cmake | 2 +- boards/parrot/bebop/default.cmake | 2 +- boards/px4/fmu-v2/default.cmake | 2 +- boards/px4/fmu-v2/fixedwing.cmake | 2 +- boards/px4/fmu-v2/lpe.cmake | 2 +- boards/px4/fmu-v2/test.cmake | 2 +- boards/px4/fmu-v3/default.cmake | 2 +- boards/px4/fmu-v3/rtps.cmake | 2 +- boards/px4/fmu-v3/stackcheck.cmake | 2 +- boards/px4/fmu-v4/default.cmake | 2 +- boards/px4/fmu-v4/rtps.cmake | 2 +- boards/px4/fmu-v4/stackcheck.cmake | 2 +- boards/px4/fmu-v4pro/default.cmake | 2 +- boards/px4/fmu-v4pro/rtps.cmake | 2 +- boards/px4/fmu-v5/default.cmake | 2 +- boards/px4/fmu-v5/fixedwing.cmake | 2 +- boards/px4/fmu-v5/multicopter.cmake | 2 +- boards/px4/fmu-v5/rtps.cmake | 2 +- boards/px4/fmu-v5/stackcheck.cmake | 2 +- boards/px4/fmu-v5x/default.cmake | 2 +- boards/px4/fmu-v5x/fixedwing.cmake | 2 +- boards/px4/fmu-v5x/multicopter.cmake | 2 +- boards/px4/fmu-v5x/rtps.cmake | 2 +- boards/px4/fmu-v5x/stackcheck.cmake | 2 +- boards/px4/raspberrypi/cross.cmake | 2 +- boards/px4/raspberrypi/native.cmake | 2 +- boards/px4/sitl/default.cmake | 2 +- boards/px4/sitl/rtps.cmake | 2 +- boards/px4/sitl/test.cmake | 2 +- msg/CMakeLists.txt | 1 + msg/airspeed_validated.msg | 10 + posix-configs/SITL/init/test/test_shutdown | 6 +- .../AirspeedValidator/AirspeedValidator.hpp | 2 +- .../CMakeLists.txt | 9 +- .../airspeed_selector_main.cpp | 601 ++++++++++++++++++ .../airspeed_selector_params.c | 93 +++ src/modules/ekf2/ekf2_main.cpp | 5 +- src/modules/logger/logger.cpp | 1 + src/modules/sensors/sensors.cpp | 6 +- .../wind_estimator/wind_estimator_main.cpp | 311 --------- .../wind_estimator/wind_estimator_params.c | 71 --- 65 files changed, 767 insertions(+), 461 deletions(-) create mode 100644 msg/airspeed_validated.msg rename src/modules/{wind_estimator => airspeed_selector}/CMakeLists.txt (93%) create mode 100644 src/modules/airspeed_selector/airspeed_selector_main.cpp create mode 100644 src/modules/airspeed_selector/airspeed_selector_params.c delete mode 100644 src/modules/wind_estimator/wind_estimator_main.cpp delete mode 100644 src/modules/wind_estimator/wind_estimator_params.c diff --git a/ROMFS/px4fmu_common/init.d-posix/1030_plane b/ROMFS/px4fmu_common/init.d-posix/1030_plane index 6a149b36b6..f847687289 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1030_plane +++ b/ROMFS/px4fmu_common/init.d-posix/1030_plane @@ -37,7 +37,6 @@ then param set RWTO_TKOFF 1 - param set WEST_EN 1 fi set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix diff --git a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol index 24a5eff04e..558737b296 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol @@ -38,7 +38,6 @@ then param set VT_MOT_COUNT 4 param set VT_TYPE 2 - param set WEST_EN 1 fi set MAV_TYPE 22 diff --git a/ROMFS/px4fmu_common/init.d-posix/1041_tailsitter b/ROMFS/px4fmu_common/init.d-posix/1041_tailsitter index dbcccec703..6128bce38e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1041_tailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/1041_tailsitter @@ -38,7 +38,6 @@ then param set VT_MOT_COUNT 0 param set VT_TYPE 0 - param set WEST_EN 1 fi set MAV_TYPE 20 diff --git a/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor index 2dc2c7271b..2a05fd6872 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor @@ -41,7 +41,6 @@ then param set VT_ELEV_MC_LOCK 0 param set VT_TYPE 1 - param set WEST_EN 1 fi set MAV_TYPE 21 diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 86e0ccc423..a1c629c276 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -164,7 +164,6 @@ then param set TRIG_INTERFACE 3 - param set WEST_EN 0 fi # Adapt timeout parameters if simulation runs faster or slower than realtime. @@ -213,10 +212,6 @@ sensors start commander start navigator start -if param compare WEST_EN 1 -then - wind_estimator start -fi if ! param compare MNT_MODE_IN -1 then diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index bcbf1fb0a5..c565aebf46 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -15,9 +15,8 @@ ekf2 start # fw_att_control start fw_pos_control_l1 start - +airspeed_selector start # # Start Land Detector. # land_detector start fixedwing - diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 10ad0cdb7c..0861bdca56 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -21,10 +21,10 @@ mc_att_control start mc_pos_control start fw_att_control start fw_pos_control_l1 start +airspeed_selector start # # Start Land Detector # Multicopter for now until we have something for VTOL # land_detector start vtol - diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 0e3cc63873..3012acca2e 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -435,14 +435,6 @@ else # navigator start - # - # Start the standalone wind estimator. - # - if param compare WEST_EN 1 - then - wind_estimator start - fi - # # Start a thermal calibration if required. # diff --git a/boards/aerotenna/ocpoc/ubuntu.cmake b/boards/aerotenna/ocpoc/ubuntu.cmake index bfe09a3c9a..0fe8e88d06 100644 --- a/boards/aerotenna/ocpoc/ubuntu.cmake +++ b/boards/aerotenna/ocpoc/ubuntu.cmake @@ -52,7 +52,7 @@ px4_add_board( #simulator vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS esc_calib diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index c35ff05a27..82d7eeb9ee 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -72,7 +72,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index b6093a1fa8..bc126e8656 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -87,7 +87,7 @@ px4_add_board( simulator vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS #bl_update diff --git a/boards/atlflight/eagle/qurt-default.cmake b/boards/atlflight/eagle/qurt-default.cmake index 5e25f06291..1b9ca0f585 100644 --- a/boards/atlflight/eagle/qurt-default.cmake +++ b/boards/atlflight/eagle/qurt-default.cmake @@ -76,7 +76,7 @@ px4_add_board( #sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS param diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index 0979f8584b..6a0f15163b 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -87,7 +87,7 @@ px4_add_board( simulator vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS #bl_update diff --git a/boards/atlflight/excelsior/qurt-default.cmake b/boards/atlflight/excelsior/qurt-default.cmake index a31a0b7523..acbd5099b9 100644 --- a/boards/atlflight/excelsior/qurt-default.cmake +++ b/boards/atlflight/excelsior/qurt-default.cmake @@ -76,7 +76,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS param diff --git a/boards/auav/x21/default.cmake b/boards/auav/x21/default.cmake index 30aa0e77a6..caa3130eb0 100644 --- a/boards/auav/x21/default.cmake +++ b/boards/auav/x21/default.cmake @@ -78,7 +78,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index c6b9177d85..243d3f3367 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -79,7 +79,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS #bl_update diff --git a/boards/beaglebone/blue/cross.cmake b/boards/beaglebone/blue/cross.cmake index 70ad59e04a..829c70993d 100644 --- a/boards/beaglebone/blue/cross.cmake +++ b/boards/beaglebone/blue/cross.cmake @@ -50,7 +50,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS esc_calib diff --git a/boards/beaglebone/blue/native.cmake b/boards/beaglebone/blue/native.cmake index 1374ad6f60..971cc47830 100644 --- a/boards/beaglebone/blue/native.cmake +++ b/boards/beaglebone/blue/native.cmake @@ -48,7 +48,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS esc_calib diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake index 11fcde6b78..95b422b433 100644 --- a/boards/bitcraze/crazyflie/default.cmake +++ b/boards/bitcraze/crazyflie/default.cmake @@ -38,7 +38,7 @@ px4_add_board( sensors sih #vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/emlid/navio2/cross.cmake b/boards/emlid/navio2/cross.cmake index 367fa6acd1..983cbac5ce 100644 --- a/boards/emlid/navio2/cross.cmake +++ b/boards/emlid/navio2/cross.cmake @@ -55,7 +55,7 @@ px4_add_board( #simulator vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS dyn diff --git a/boards/emlid/navio2/native.cmake b/boards/emlid/navio2/native.cmake index 781ad8f32b..0060fa923a 100644 --- a/boards/emlid/navio2/native.cmake +++ b/boards/emlid/navio2/native.cmake @@ -53,7 +53,7 @@ px4_add_board( #simulator vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS dyn diff --git a/boards/intel/aerofc-v1/default.cmake b/boards/intel/aerofc-v1/default.cmake index ce0d07d9cc..21c0fdffa9 100644 --- a/boards/intel/aerofc-v1/default.cmake +++ b/boards/intel/aerofc-v1/default.cmake @@ -55,7 +55,7 @@ px4_add_board( sih vmount #vtol_att_control - #wind_estimator + #airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/intel/aerofc-v1/rtps.cmake b/boards/intel/aerofc-v1/rtps.cmake index 123cadfd8d..0310c2bc1f 100644 --- a/boards/intel/aerofc-v1/rtps.cmake +++ b/boards/intel/aerofc-v1/rtps.cmake @@ -59,7 +59,7 @@ px4_add_board( sih vmount #vtol_att_control - #wind_estimator + #airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index bf64cde89b..469e90e971 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -74,7 +74,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake index 3a6f449023..3de57236d4 100644 --- a/boards/omnibus/f4sd/default.cmake +++ b/boards/omnibus/f4sd/default.cmake @@ -67,7 +67,7 @@ px4_add_board( sih #vmount #vtol_att_control - #wind_estimator + #airspeed_selector SYSTEMCMDS #bl_update diff --git a/boards/parrot/bebop/default.cmake b/boards/parrot/bebop/default.cmake index 4b8d01ee68..6446169931 100644 --- a/boards/parrot/bebop/default.cmake +++ b/boards/parrot/bebop/default.cmake @@ -41,7 +41,7 @@ px4_add_board( sensors sih #vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS #config diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index a73d2fcc6b..b218dcb38f 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -86,7 +86,7 @@ px4_add_board( sensors vmount vtol_att_control - #wind_estimator + #airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v2/fixedwing.cmake b/boards/px4/fmu-v2/fixedwing.cmake index f0a6355cd6..5af5a9db9a 100644 --- a/boards/px4/fmu-v2/fixedwing.cmake +++ b/boards/px4/fmu-v2/fixedwing.cmake @@ -58,7 +58,7 @@ px4_add_board( navigator sensors vmount - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v2/lpe.cmake b/boards/px4/fmu-v2/lpe.cmake index 56b9168123..3100902268 100644 --- a/boards/px4/fmu-v2/lpe.cmake +++ b/boards/px4/fmu-v2/lpe.cmake @@ -82,7 +82,7 @@ px4_add_board( sensors vmount #vtol_att_control - #wind_estimator + #airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake index 52f1842229..39b89893c8 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -82,7 +82,7 @@ px4_add_board( sensors vmount vtol_att_control - #wind_estimator + #airspeed_selector SYSTEMCMDS #bl_update diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index 9deece41a4..05e87e5e96 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -86,7 +86,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index 1e4a4a4305..c08515a298 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -86,7 +86,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake index 9f10338263..4ff91eb9b0 100644 --- a/boards/px4/fmu-v3/stackcheck.cmake +++ b/boards/px4/fmu-v3/stackcheck.cmake @@ -85,7 +85,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 4ca73855bb..5599773bec 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -70,7 +70,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index a7f747616d..bd423dc2f2 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -73,7 +73,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index 7ea6552244..6c853978b4 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -70,7 +70,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index b8f2671834..e229eb12d6 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -84,7 +84,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index 65dab30976..0de0625ea6 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -84,7 +84,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index da482d7fcf..797c6e01e0 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -84,7 +84,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index a5a9a7103c..8eff52df13 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -58,7 +58,7 @@ px4_add_board( navigator sensors vmount - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index a0aa28ce43..4fdb8867e1 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -67,7 +67,7 @@ px4_add_board( sensors sih vmount - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index 87da8279ef..b35172fb1b 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -83,7 +83,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index a93567e1da..db3018de8e 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -83,7 +83,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index 054b543564..9529c26ae1 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -84,7 +84,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v5x/fixedwing.cmake b/boards/px4/fmu-v5x/fixedwing.cmake index 4a6a74e2d5..e05bd5f446 100644 --- a/boards/px4/fmu-v5x/fixedwing.cmake +++ b/boards/px4/fmu-v5x/fixedwing.cmake @@ -68,7 +68,7 @@ px4_add_board( navigator sensors vmount - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v5x/multicopter.cmake b/boards/px4/fmu-v5x/multicopter.cmake index 5e37e8cedd..49eba6150c 100644 --- a/boards/px4/fmu-v5x/multicopter.cmake +++ b/boards/px4/fmu-v5x/multicopter.cmake @@ -74,7 +74,7 @@ px4_add_board( sensors sih vmount - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v5x/rtps.cmake b/boards/px4/fmu-v5x/rtps.cmake index 975527927f..c3126a0222 100644 --- a/boards/px4/fmu-v5x/rtps.cmake +++ b/boards/px4/fmu-v5x/rtps.cmake @@ -85,7 +85,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v5x/stackcheck.cmake b/boards/px4/fmu-v5x/stackcheck.cmake index 8e1c010ec9..32799b48f8 100644 --- a/boards/px4/fmu-v5x/stackcheck.cmake +++ b/boards/px4/fmu-v5x/stackcheck.cmake @@ -84,7 +84,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/raspberrypi/cross.cmake b/boards/px4/raspberrypi/cross.cmake index 04c5a723a0..b999eac7fd 100644 --- a/boards/px4/raspberrypi/cross.cmake +++ b/boards/px4/raspberrypi/cross.cmake @@ -47,7 +47,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS dyn diff --git a/boards/px4/raspberrypi/native.cmake b/boards/px4/raspberrypi/native.cmake index da7bb77090..e21cbaa3ca 100644 --- a/boards/px4/raspberrypi/native.cmake +++ b/boards/px4/raspberrypi/native.cmake @@ -46,7 +46,7 @@ px4_add_board( #simulator vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS dyn diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index b7faf27a45..ed04be1010 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -45,7 +45,7 @@ px4_add_board( simulator vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS #bl_update diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index 89d84b1563..0832820bcc 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -46,7 +46,7 @@ px4_add_board( simulator vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS #bl_update diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index 1e270c641c..3fc12e8755 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -45,7 +45,7 @@ px4_add_board( simulator vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS #bl_update diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 67ea1d8605..ae38338d05 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -38,6 +38,7 @@ set(msg_files actuator_outputs.msg adc_report.msg airspeed.msg + airspeed_validated.msg battery_status.msg camera_capture.msg camera_trigger.msg diff --git a/msg/airspeed_validated.msg b/msg/airspeed_validated.msg new file mode 100644 index 0000000000..7b451d7b3c --- /dev/null +++ b/msg/airspeed_validated.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) + +float32 indicated_airspeed_m_s # indicated airspeed in m/s (IAS), set to NAN if invalid +float32 equivalent_airspeed_m_s # equivalent airspeed in m/s (accounts for instrumentation errors) (EAS), set to NAN if invalid +float32 true_airspeed_m_s # true filtered airspeed in m/s (TAS), set to NAN if invalid + +float32 true_ground_minus_wind_m_s # TAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid +float32 indicated_ground_minus_wind_m_s # IAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid + +int8 selected_airspeed_index # 0-2: airspeed sensor index, -1: groundspeed-windspeed, -2: airspeed invalid diff --git a/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown index 8c6d76b611..6c742afd07 100644 --- a/posix-configs/SITL/init/test/test_shutdown +++ b/posix-configs/SITL/init/test/test_shutdown @@ -29,7 +29,7 @@ mc_pos_control start mc_att_control start fw_pos_control_l1 start fw_att_control start -wind_estimator start +airspeed_selector start #mixer load /dev/pwm_output0 ROMFS/sitl/mixers/standard_vtol_sitl.main.mix @@ -56,7 +56,7 @@ mc_pos_control status mc_att_control status fw_pos_control_l1 status fw_att_control status -wind_estimator status +airspeed_selector status dataman status uorb status @@ -72,7 +72,7 @@ navigator stop commander stop land_detector stop ekf2 stop -wind_estimator stop +airspeed_selector stop sensors stop sleep 2 diff --git a/src/lib/AirspeedValidator/AirspeedValidator.hpp b/src/lib/AirspeedValidator/AirspeedValidator.hpp index 76b31831c4..126a6efe86 100644 --- a/src/lib/AirspeedValidator/AirspeedValidator.hpp +++ b/src/lib/AirspeedValidator/AirspeedValidator.hpp @@ -99,7 +99,7 @@ public: void set_wind_estimator_beta_gate(uint8_t gate_size) { _wind_estimator.set_beta_gate(gate_size); } void set_wind_estimator_scale_estimation_on(bool scale_estimation_on) {_wind_estimator_scale_estimation_on = scale_estimation_on;} - void set_airspeed_scale(float airspeed_scale_manual) { _wind_estimator.enforce_airspeed_scale(airspeed_scale_manual);} + void set_airspeed_scale(float airspeed_scale_manual) { _wind_estimator.enforce_airspeed_scale(1.0f / airspeed_scale_manual);} // scale is inverted inside the wind estimator // setters for failure detection tuning parameters void set_tas_innov_threshold(float tas_innov_threshold) { _tas_innov_threshold = tas_innov_threshold; } diff --git a/src/modules/wind_estimator/CMakeLists.txt b/src/modules/airspeed_selector/CMakeLists.txt similarity index 93% rename from src/modules/wind_estimator/CMakeLists.txt rename to src/modules/airspeed_selector/CMakeLists.txt index 3f6bff2d4f..6c2ad20cf4 100644 --- a/src/modules/wind_estimator/CMakeLists.txt +++ b/src/modules/airspeed_selector/CMakeLists.txt @@ -31,13 +31,12 @@ # ############################################################################ px4_add_module( - MODULE modules__wind_estimator - MAIN wind_estimator - INCLUDES - ${PX4_SOURCE_DIR}/src/lib/ecl + MODULE modules__airspeed_selector + MAIN airspeed_selector SRCS - wind_estimator_main.cpp + airspeed_selector_main.cpp DEPENDS git_ecl ecl_airdata + AirspeedValidator ) diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp new file mode 100644 index 0000000000..f483d7e30e --- /dev/null +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -0,0 +1,601 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#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 + +using namespace time_literals; + +static constexpr uint32_t SCHEDULE_INTERVAL{100_ms}; /**< The schedule interval in usec (10 Hz) */ + +using matrix::Dcmf; +using matrix::Quatf; +using matrix::Vector2f; +using matrix::Vector3f; + +class AirspeedModule : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + + AirspeedModule(); + + ~AirspeedModule(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /* run the main loop */ + void Run() override; + + int print_status() override; + +private: + static constexpr int MAX_NUM_AIRSPEED_SENSORS = 3; /**< Support max 3 airspeed sensors */ + + orb_advert_t _airspeed_validated_pub {nullptr}; /**< airspeed validated topic*/ + orb_advert_t _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */ + orb_advert_t _mavlink_log_pub {nullptr}; /**< mavlink log topic*/ + + uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; + uORB::Subscription _param_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; + + estimator_status_s _estimator_status {}; + parameter_update_s _parameter_update {}; + vehicle_acceleration_s _accel {}; + vehicle_air_data_s _vehicle_air_data {}; + vehicle_attitude_s _vehicle_attitude {}; + vehicle_land_detected_s _vehicle_land_detected {}; + vehicle_local_position_s _vehicle_local_position {}; + vehicle_status_s _vehicle_status {}; + vtol_vehicle_status_s _vtol_vehicle_status {}; + + WindEstimator _wind_estimator_sideslip; /**< wind estimator instance only fusing sideslip */ + wind_estimate_s _wind_estimate_sideslip {}; /**< wind estimate message for wind estimator instance only fusing sideslip */ + + int _airspeed_sub[MAX_NUM_AIRSPEED_SENSORS] {}; /**< raw airspeed topics subscriptions. Max 3 airspeeds sensors. */ + int _number_of_airspeed_sensors{0}; /**< number of airspeed sensors in use (detected during initialization)*/ + AirspeedValidator *_airspeed_validator{nullptr}; /**< airspeedValidator instances (one for each sensor, assigned dynamically during startup) */ + + int _valid_airspeed_index{-1}; /**< index of currently chosen (valid) airspeed sensor */ + int _prev_airspeed_index{-1}; /**< previously chosen airspeed sensor index */ + bool _initialized{false}; /**< module initialized*/ + bool _vehicle_local_position_valid{false}; /**< local position (from GPS) valid */ + bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */ + float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */ + float _ground_minus_wind_EAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */ + + bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */ + + perf_counter_t _perf_elapsed{}; + perf_counter_t _perf_interval{}; + + + DEFINE_PARAMETERS( + (ParamFloat) _param_west_w_p_noise, + (ParamFloat) _param_west_sc_p_noise, + (ParamFloat) _param_west_tas_noise, + (ParamFloat) _param_west_beta_noise, + (ParamInt) _param_west_tas_gate, + (ParamInt) _param_west_beta_gate, + (ParamInt) _param_west_scale_estimation_on, + (ParamFloat) _param_west_airspeed_scale, + + + (ParamFloat) _tas_innov_threshold, /**< innovation check threshold */ + (ParamFloat) _tas_innov_integ_threshold, /**< innovation check integrator threshold */ + (ParamInt) _checks_fail_delay, /**< delay to declare airspeed invalid */ + (ParamInt) _checks_clear_delay, /**< delay to declare airspeed valid again */ + (ParamFloat) _airspeed_stall /**< stall speed*/ + ) + + int start(); + void update_params(); /**< update parameters */ + void poll_topics(); /**< poll all topics required beside airspeed (e.g. current temperature) */ + void update_wind_estimator_sideslip(); /**< update the wind estimator instance only fusing sideslip */ + void update_ground_minus_wind_airspeed(); /**< update airspeed estimate based on groundspeed minus windspeed */ + void select_airspeed_and_publish(); /**< select airspeed sensor (or groundspeed-windspeed) */ + void publish_wind_estimates(); /**< publish wind estimator states (from all wind estimators running) */ + +}; + +AirspeedModule::AirspeedModule(): + ModuleParams(nullptr), + ScheduledWorkItem(px4::wq_configurations::lp_default) +{ + // initialise parameters + update_params(); + + _perf_elapsed = perf_alloc_once(PC_ELAPSED, "airspeed_selector elapsed"); + _perf_interval = perf_alloc_once(PC_INTERVAL, "airspeed_selector interval"); +} + +AirspeedModule::~AirspeedModule() +{ + ScheduleClear(); + + for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) { + if (_wind_est_pub[i] != nullptr) { + orb_unadvertise(_wind_est_pub[i]); + } + } + + orb_unadvertise(_airspeed_validated_pub); + + + perf_free(_perf_elapsed); + perf_free(_perf_interval); + + if (_airspeed_validator != nullptr) { + delete[] _airspeed_validator; + } +} + +int +AirspeedModule::task_spawn(int argc, char *argv[]) +{ + AirspeedModule *dev = new AirspeedModule(); + + // check if the trampoline is called for the first time + if (!dev) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _object.store(dev); + + if (dev) { + dev->ScheduleOnInterval(SCHEDULE_INTERVAL, 10000); + _task_id = task_id_is_work_queue; + return PX4_OK; + } + + return PX4_ERROR; +} + +void +AirspeedModule::Run() +{ + perf_count(_perf_interval); + perf_begin(_perf_elapsed); + + /* the first time we run through here, initialize N airspeedValidator + * instances (N = number of airspeed sensors detected) + */ + if (!_initialized) { + for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) { + if (orb_exists(ORB_ID(airspeed), i) != 0) { + continue; + } + + _number_of_airspeed_sensors = i + 1; + } + + _airspeed_validator = new AirspeedValidator[_number_of_airspeed_sensors]; + + if (_number_of_airspeed_sensors > 0) { + for (int i = 0; i < _number_of_airspeed_sensors; i++) { + _airspeed_sub[i] = orb_subscribe_multi(ORB_ID(airspeed), i); + _valid_airspeed_index = 0; // set index to first sensor + _prev_airspeed_index = 0; // set index to first sensor + } + } + + _initialized = true; + } + + parameter_update_s update; + + if (_param_sub.update(&update)) { + update_params(); + } + + poll_topics(); + update_wind_estimator_sideslip(); + update_ground_minus_wind_airspeed(); + + if (_airspeed_validator != nullptr) { + + bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + bool fixed_wing = !_vtol_vehicle_status.vtol_in_rw_mode; + bool in_air = !_vehicle_land_detected.landed; + + /* Prepare data for airspeed_validator */ + struct airspeed_validator_update_data input_data = {}; + input_data.timestamp = hrt_absolute_time(); + input_data.lpos_vx = _vehicle_local_position.vx; + input_data.lpos_vy = _vehicle_local_position.vy; + input_data.lpos_vz = _vehicle_local_position.vz; + input_data.lpos_valid = _vehicle_local_position_valid; + input_data.lpos_evh = _vehicle_local_position.evh; + input_data.lpos_evv = _vehicle_local_position.evv; + input_data.att_q[0] = _vehicle_attitude.q[0]; + input_data.att_q[1] = _vehicle_attitude.q[1]; + input_data.att_q[2] = _vehicle_attitude.q[2]; + input_data.att_q[3] = _vehicle_attitude.q[3]; + input_data.air_pressure_pa = _vehicle_air_data.baro_pressure_pa; + input_data.accel_z = _accel.xyz[2]; + input_data.vel_test_ratio = _estimator_status.vel_test_ratio; + input_data.mag_test_ratio = _estimator_status.mag_test_ratio; + + /* iterate through all airspeed sensors, poll new data from them and update their validators */ + for (int i = 0; i < _number_of_airspeed_sensors; i++) { + + /* poll airspeed data */ + airspeed_s airspeed_raw = {}; + orb_copy(ORB_ID(airspeed), _airspeed_sub[i], &airspeed_raw); // poll raw airspeed topic of the i-th sensor + input_data.airspeed_indicated_raw = airspeed_raw.indicated_airspeed_m_s; + input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s; + input_data.airspeed_timestamp = airspeed_raw.timestamp; + input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius; + + /* update in_fixed_wing_flight for the current airspeed sensor validator */ + /* takeoff situation is active from start till one of the sensors' IAS or groundspeed_EAS is above stall speed */ + if (airspeed_raw.indicated_airspeed_m_s > _airspeed_stall.get() || _ground_minus_wind_EAS > _airspeed_stall.get()) { + _in_takeoff_situation = false; + } + + /* reset takeoff_situation to true when not in air or not in fixed-wing mode */ + if (!in_air || !fixed_wing) { + _in_takeoff_situation = true; + } + + input_data.in_fixed_wing_flight = (armed && fixed_wing && in_air && !_in_takeoff_situation); + + /* push input data into airspeed validator */ + _airspeed_validator[i].update_airspeed_validator(input_data); + + } + } + + select_airspeed_and_publish(); + + perf_end(_perf_elapsed); + + if (should_exit()) { + exit_and_cleanup(); + } +} + + +void AirspeedModule::update_params() +{ + updateParams(); + + /* update wind estimator (sideslip fusion only) parameters */ + _wind_estimator_sideslip.set_wind_p_noise(_param_west_w_p_noise.get()); + _wind_estimator_sideslip.set_tas_scale_p_noise(_param_west_sc_p_noise.get()); + _wind_estimator_sideslip.set_tas_noise(_param_west_tas_noise.get()); + _wind_estimator_sideslip.set_beta_noise(_param_west_beta_noise.get()); + _wind_estimator_sideslip.set_tas_gate(_param_west_tas_gate.get()); + _wind_estimator_sideslip.set_beta_gate(_param_west_beta_gate.get()); + + /* update airspeedValidator parameters */ + for (int i = 0; i < _number_of_airspeed_sensors; i++) { + _airspeed_validator[i].set_wind_estimator_wind_p_noise(_param_west_w_p_noise.get()); + _airspeed_validator[i].set_wind_estimator_tas_scale_p_noise(_param_west_sc_p_noise.get()); + _airspeed_validator[i].set_wind_estimator_tas_noise(_param_west_tas_noise.get()); + _airspeed_validator[i].set_wind_estimator_beta_noise(_param_west_beta_noise.get()); + _airspeed_validator[i].set_wind_estimator_tas_gate(_param_west_tas_gate.get()); + _airspeed_validator[i].set_wind_estimator_beta_gate(_param_west_beta_gate.get()); + _airspeed_validator[i].set_wind_estimator_scale_estimation_on(_param_west_scale_estimation_on.get()); + + /* Only apply manual entered airspeed scale to first airspeed measurement */ + _airspeed_validator[0].set_airspeed_scale(_param_west_airspeed_scale.get()); + + _airspeed_validator[i].set_tas_innov_threshold(_tas_innov_threshold.get()); + _airspeed_validator[i].set_tas_innov_integ_threshold(_tas_innov_integ_threshold.get()); + _airspeed_validator[i].set_checks_fail_delay(_checks_fail_delay.get()); + _airspeed_validator[i].set_checks_clear_delay(_checks_clear_delay.get()); + _airspeed_validator[i].set_airspeed_stall(_airspeed_stall.get()); + } + + /* when airspeed scale estimation is turned on and the airspeed is valid, then set the scale inside the wind estimator to -1 such that it starts to estimate it */ + if (!_scale_estimation_previously_on && _param_west_scale_estimation_on.get()) { + if (_valid_airspeed_index >= 0) { + _airspeed_validator[0].set_airspeed_scale( + -1.0f); // set it to a negative value to start estimation inside wind estimator + + } else { + mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor."); + _param_west_scale_estimation_on.set(0); // reset this param to 0 as estimation was not turned on + _param_west_scale_estimation_on.commit_no_notification(); + } + + /* If one sensor is valid and we switched out of scale estimation, then publish message and change the value of param ARSP_ARSP_SCALE */ + + } else if (_scale_estimation_previously_on && !_param_west_scale_estimation_on.get()) { + if (_valid_airspeed_index >= 0) { + + _param_west_airspeed_scale.set(_airspeed_validator[_valid_airspeed_index].get_EAS_scale()); + _param_west_airspeed_scale.commit_no_notification(); + _airspeed_validator[_valid_airspeed_index].set_airspeed_scale(_param_west_airspeed_scale.get()); + + mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ARSP_ARSP_SCALE): %0.2f", + (double)_airspeed_validator[_valid_airspeed_index].get_EAS_scale()); + + } else { + mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor."); + } + } + + _scale_estimation_previously_on = _param_west_scale_estimation_on.get(); + +} + +void AirspeedModule::poll_topics() +{ + _estimator_status_sub.update(&_estimator_status); + _vehicle_acceleration_sub.update(&_accel); + _vehicle_air_data_sub.update(&_vehicle_air_data); + _vehicle_attitude_sub.update(&_vehicle_attitude); + _vehicle_land_detected_sub.update(&_vehicle_land_detected); + _vehicle_status_sub.update(&_vehicle_status); + _vtol_vehicle_status_sub.update(&_vtol_vehicle_status); + _vehicle_local_position_sub.update(&_vehicle_local_position); + + _vehicle_local_position_valid = (hrt_absolute_time() - _vehicle_local_position.timestamp < 1_s) + && (_vehicle_local_position.timestamp > 0) && _vehicle_local_position.v_xy_valid; + + + +} + +void AirspeedModule::update_wind_estimator_sideslip() +{ + bool att_valid = true; // TODO: check if attitude is valid + const hrt_abstime time_now_usec = hrt_absolute_time(); + + /* update wind and airspeed estimator */ + _wind_estimator_sideslip.update(time_now_usec); + + if (_vehicle_local_position_valid && att_valid) { + Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz); + Quatf q(_vehicle_attitude.q); + + /* sideslip fusion */ + _wind_estimator_sideslip.fuse_beta(time_now_usec, vI, q); + } + + /* fill message for publishing later */ + _wind_estimate_sideslip.timestamp = time_now_usec; + float wind[2]; + _wind_estimator_sideslip.get_wind(wind); + _wind_estimate_sideslip.windspeed_north = wind[0]; + _wind_estimate_sideslip.windspeed_east = wind[1]; + float wind_cov[2]; + _wind_estimator_sideslip.get_wind_var(wind_cov); + _wind_estimate_sideslip.variance_north = wind_cov[0]; + _wind_estimate_sideslip.variance_east = wind_cov[1]; + _wind_estimate_sideslip.tas_innov = _wind_estimator_sideslip.get_tas_innov(); + _wind_estimate_sideslip.tas_innov_var = _wind_estimator_sideslip.get_tas_innov_var(); + _wind_estimate_sideslip.beta_innov = _wind_estimator_sideslip.get_beta_innov(); + _wind_estimate_sideslip.beta_innov_var = _wind_estimator_sideslip.get_beta_innov_var(); + _wind_estimate_sideslip.tas_scale = _wind_estimator_sideslip.get_tas_scale(); +} + +void AirspeedModule::update_ground_minus_wind_airspeed() +{ + /* calculate airspeed estimate based on groundspeed-windspeed to use as fallback */ + float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north; + float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east; + float TAS_down = _vehicle_local_position.vz; // no wind estimate in z + _ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down); + _ground_minus_wind_EAS = calc_EAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa, + _vehicle_air_data.baro_temp_celcius); +} + + +void AirspeedModule::select_airspeed_and_publish() +{ + /* airspeed index: + / 0: first airspeed sensor valid + / 1: second airspeed sensor valid + / -1: airspeed sensor(s) invalid, but groundspeed-windspeed estimate valid + / -2: airspeed invalid (sensors and groundspeed-windspeed estimate invalid) + */ + bool find_new_valid_index = false; + + /* find new valid index if airspeed currently invalid (but we have sensors) */ + if ((_number_of_airspeed_sensors > 0 && _prev_airspeed_index < 0) || + (_prev_airspeed_index >= 0 && !_airspeed_validator[_prev_airspeed_index].get_airspeed_valid()) || + _prev_airspeed_index == -2) { + + find_new_valid_index = true; + } + + if (find_new_valid_index) { + _valid_airspeed_index = -1; + + for (int i = 0; i < _number_of_airspeed_sensors; i++) { + if (_airspeed_validator[i].get_airspeed_valid()) { + _valid_airspeed_index = i; + break; + } + } + } + + if (_valid_airspeed_index < 0 && !_vehicle_local_position_valid) { + _valid_airspeed_index = -2; + } + + /* publish critical message (and log) in index has changed */ + if (_valid_airspeed_index != _prev_airspeed_index) { + mavlink_log_critical(&_mavlink_log_pub, "Airspeed: switched from sensor %i to %i", _prev_airspeed_index, + _valid_airspeed_index); + } + + _prev_airspeed_index = _valid_airspeed_index; + + /* fill out airspeed_validated message for publishing it */ + airspeed_validated_s airspeed_validated = {}; + airspeed_validated.timestamp = hrt_absolute_time(); + airspeed_validated.true_ground_minus_wind_m_s = NAN; + airspeed_validated.indicated_ground_minus_wind_m_s = NAN; + airspeed_validated.indicated_airspeed_m_s = NAN; + airspeed_validated.equivalent_airspeed_m_s = NAN; + airspeed_validated.true_airspeed_m_s = NAN; + + airspeed_validated.selected_airspeed_index = _valid_airspeed_index; + + switch (_valid_airspeed_index) { + case -2: + break; + + case -1: + airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS; + airspeed_validated.indicated_ground_minus_wind_m_s = _ground_minus_wind_EAS; + break; + + default: + airspeed_validated.indicated_airspeed_m_s = _airspeed_validator[_valid_airspeed_index].get_IAS(); + airspeed_validated.equivalent_airspeed_m_s = _airspeed_validator[_valid_airspeed_index].get_EAS(); + airspeed_validated.true_airspeed_m_s = _airspeed_validator[_valid_airspeed_index].get_TAS(); + airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS; + airspeed_validated.indicated_ground_minus_wind_m_s = _ground_minus_wind_EAS; + break; + } + + /* publish airspeed validated topic */ + int instance; + orb_publish_auto(ORB_ID(airspeed_validated), &_airspeed_validated_pub, &airspeed_validated, &instance, + ORB_PRIO_DEFAULT); + + /* publish sideslip-only-fusion wind topic */ + orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub[0], &_wind_estimate_sideslip, &instance, ORB_PRIO_LOW); + + /* publish the wind estimator states from all airspeed validators */ + for (int i = 0; i < _number_of_airspeed_sensors; i++) { + wind_estimate_s wind_est = _airspeed_validator[i].get_wind_estimator_states(hrt_absolute_time()); + orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub[i + 1], &wind_est, &instance, ORB_PRIO_LOW); + } + +} + +int AirspeedModule::custom_command(int argc, char *argv[]) +{ + if (!is_running()) { + int ret = AirspeedModule::task_spawn(argc, argv); + + if (ret) { + return ret; + } + } + + return print_usage("unknown command"); +} + +int AirspeedModule::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module provides a single airspeed_validated topic, containing an indicated (IAS), +equivalend (EAS), true airspeed (TAS) and the information if the estimation currently +is invalid and if based sensor readings or on groundspeed minus windspeed. +Supporting the input of multiple "raw" airspeed inputs, this module automatically switches +to a valid sensor in case of failure detection. For failure detection as well as for +the estimation of a scale factor from IAS to EAS, it runs several wind estimators +and also publishes those. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("airspeed_estimator", "estimator"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int AirspeedModule::print_status() +{ + perf_print_counter(_perf_elapsed); + perf_print_counter(_perf_interval); + + int instance = 0; + uORB::SubscriptionData est{ORB_ID(airspeed_validated), (uint8_t)instance}; + est.update(); + PX4_INFO("Number of airspeed sensors: %i", _number_of_airspeed_sensors); + print_message(est.get()); + + return 0; +} + +extern "C" __EXPORT int airspeed_selector_main(int argc, char *argv[]); + +int +airspeed_selector_main(int argc, char *argv[]) +{ + return AirspeedModule::main(argc, argv); +} diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c new file mode 100644 index 0000000000..69572318eb --- /dev/null +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -0,0 +1,93 @@ + +/** + * Airspeed Selector: Wind estimator wind process noise + * + * Wind process noise of the internal wind estimator(s) of the airspeed selector. + * + * @min 0 + * @max 1 + * @unit m/s/s + * @group Airspeed Validator + */ +PARAM_DEFINE_FLOAT(ARSP_W_P_NOISE, 0.1f); + +/** + * Airspeed Selector: Wind estimator true airspeed scale process noise + * + * Airspeed scale process noise of the internal wind estimator(s) of the airspeed selector. + * + * @min 0 + * @max 0.1 + * @unit 1/s + * @group Airspeed Validator + */ +PARAM_DEFINE_FLOAT(ARSP_SC_P_NOISE, 0.0001); + +/** + * Airspeed Selector: Wind estimator true airspeed measurement noise + * + * True airspeed measurement noise of the internal wind estimator(s) of the airspeed selector. + * + * @min 0 + * @max 4 + * @unit m/s + * @group Airspeed Validator + */ +PARAM_DEFINE_FLOAT(ARSP_TAS_NOISE, 1.4); + +/** + * Airspeed Selector: Wind estimator sideslip measurement noise + * + * Sideslip measurement noise of the internal wind estimator(s) of the airspeed selector. + * + * @min 0 + * @max 1 + * @unit rad + * @group Airspeed Validator + */ +PARAM_DEFINE_FLOAT(ARSP_BETA_NOISE, 0.3); + +/** + * Airspeed Selector: Gate size for true airspeed fusion + * + * Sets the number of standard deviations used by the innovation consistency test. + * + * @min 1 + * @max 5 + * @unit SD + * @group Airspeed Validator + */ +PARAM_DEFINE_INT32(ARSP_TAS_GATE, 3); + +/** + * Airspeed Selector: Gate size for true sideslip fusion + * + * Sets the number of standard deviations used by the innovation consistency test. + * + * @min 1 + * @max 5 + * @unit SD + * @group Airspeed Validator + */ +PARAM_DEFINE_INT32(ARSP_BETA_GATE, 1); + +/** + * Automatic airspeed scale estimation on + * + * Turns the automatic airspeed scale (scale from IAS to CAS/EAS) on or off. It is recommended level (keeping altitude) while performing the estimation. Set to 1 to start estimation (best when already flying). Set to 0 to end scale estimation. The estimated scale is then saved in the ARSP_ARSP_SCALE parameter. + * + * @boolean + * @group Airspeed Validator + */ +PARAM_DEFINE_INT32(ARSP_SCALE_EST, 0); + +/** + * Airspeed scale (scale from IAS to CAS/EAS) + * + * Scale can either be entered manually, or estimated in-flight by setting ARSP_SCALE_EST to 1. + * + * @min 0.5 + * @max 1.5 + * @group Airspeed Validator + */ +PARAM_DEFINE_FLOAT(ARSP_ARSP_SCALE, 1.0f); diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 60a7792a2b..d558db09db 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -283,7 +283,7 @@ private: uORB::Publication _sensor_bias_pub{ORB_ID(sensor_bias)}; uORB::Publication _att_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; - uORB::Publication _wind_pub{ORB_ID(wind_estimate)}; + uORB::Publication _wind_pub{ORB_ID(wind_estimate), ORB_PRIO_DEFAULT}; uORB::PublicationData _vehicle_global_position_pub{ORB_ID(vehicle_global_position)}; uORB::PublicationData _vehicle_local_position_pub{ORB_ID(vehicle_local_position)}; @@ -1764,12 +1764,13 @@ bool Ekf2::publish_wind_estimate(const hrt_abstime ×tamp) _ekf.get_wind_velocity_var(wind_var); // Publish wind estimate - wind_estimate_s wind_estimate; + wind_estimate_s wind_estimate{}; wind_estimate.timestamp = timestamp; wind_estimate.windspeed_north = velNE_wind[0]; wind_estimate.windspeed_east = velNE_wind[1]; wind_estimate.variance_north = wind_var[0]; wind_estimate.variance_east = wind_var[1]; + wind_estimate.tas_scale = 1.0f; //fix to 1 as scale not estimated in ekf _wind_pub.publish(wind_estimate); diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 13673b7502..95b85e8879 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -511,6 +511,7 @@ void Logger::add_default_topics() add_topic("actuator_controls_0", 100); add_topic("actuator_controls_1", 100); add_topic("airspeed", 200); + add_topic("airspeed_validated", 200); add_topic("camera_capture"); add_topic("camera_trigger"); add_topic("camera_trigger_secondary"); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d4f0a3f279..d28de3647e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -346,14 +346,14 @@ Sensors::diff_pres_poll(const vehicle_air_data_s &raw) } /* don't risk to feed negative airspeed into the system */ - airspeed.indicated_airspeed_m_s = calc_indicated_airspeed_corrected((enum AIRSPEED_COMPENSATION_MODEL) + airspeed.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL) _parameters.air_cmodel, smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm, diff_pres.differential_pressure_filtered_pa, raw.baro_pressure_pa, air_temperature_celsius); - airspeed.true_airspeed_m_s = calc_true_airspeed_from_indicated(airspeed.indicated_airspeed_m_s, raw.baro_pressure_pa, - air_temperature_celsius); + airspeed.true_airspeed_m_s = calc_TAS_from_EAS(airspeed.indicated_airspeed_m_s, raw.baro_pressure_pa, + air_temperature_celsius); // assume that EAS = IAS as we don't have an EAS-scale here airspeed.air_temperature_celsius = air_temperature_celsius; diff --git a/src/modules/wind_estimator/wind_estimator_main.cpp b/src/modules/wind_estimator/wind_estimator_main.cpp deleted file mode 100644 index 65cddf1dc2..0000000000 --- a/src/modules/wind_estimator/wind_estimator_main.cpp +++ /dev/null @@ -1,311 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 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. - * - ****************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -static constexpr uint32_t SCHEDULE_INTERVAL{100_ms}; /**< The schedule interval in usec (10 Hz) */ - -using matrix::Dcmf; -using matrix::Quatf; -using matrix::Vector2f; -using matrix::Vector3f; - -class WindEstimatorModule : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem -{ -public: - - WindEstimatorModule(); - - ~WindEstimatorModule(); - - /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); - - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); - - /** @see ModuleBase */ - static int print_usage(const char *reason = nullptr); - - // run the main loop - void Run() override; - - int print_status() override; - -private: - - WindEstimator _wind_estimator; - orb_advert_t _wind_est_pub{nullptr}; /**< wind estimate topic */ - - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; - uORB::Subscription _param_sub{ORB_ID(parameter_update)}; - - perf_counter_t _perf_elapsed{}; - perf_counter_t _perf_interval{}; - - int _instance{-1}; - - DEFINE_PARAMETERS( - (ParamFloat) _param_west_w_p_noise, - (ParamFloat) _param_west_sc_p_noise, - (ParamFloat) _param_west_tas_noise, - (ParamFloat) _param_west_beta_noise, - (ParamInt) _param_west_tas_gate, - (ParamInt) _param_west_beta_gate - ) - - int start(); - - void update_params(); - - bool subscribe_topics(); -}; - -WindEstimatorModule::WindEstimatorModule(): - ModuleParams(nullptr), - ScheduledWorkItem(px4::wq_configurations::lp_default) -{ - // initialise parameters - update_params(); - - _perf_elapsed = perf_alloc_once(PC_ELAPSED, "wind_estimator elapsed"); - _perf_interval = perf_alloc_once(PC_INTERVAL, "wind_estimator interval"); -} - -WindEstimatorModule::~WindEstimatorModule() -{ - ScheduleClear(); - - orb_unadvertise(_wind_est_pub); - - perf_free(_perf_elapsed); - perf_free(_perf_interval); -} - -int -WindEstimatorModule::task_spawn(int argc, char *argv[]) -{ - /* schedule a cycle to start things */ - WindEstimatorModule *dev = new WindEstimatorModule(); - - // check if the trampoline is called for the first time - if (!dev) { - PX4_ERR("alloc failed"); - return PX4_ERROR; - } - - _object.store(dev); - - if (dev) { - dev->ScheduleOnInterval(SCHEDULE_INTERVAL, 10000); - _task_id = task_id_is_work_queue; - return PX4_OK; - } - - return PX4_ERROR; -} - -void -WindEstimatorModule::Run() -{ - perf_count(_perf_interval); - perf_begin(_perf_elapsed); - - parameter_update_s param{}; - - if (_param_sub.update(¶m)) { - update_params(); - } - - bool lpos_valid = false; - bool att_valid = false; - - const hrt_abstime time_now_usec = hrt_absolute_time(); - - // validate required conditions for the filter to fuse measurements - - vehicle_attitude_s att{}; - - if (_vehicle_attitude_sub.copy(&att)) { - att_valid = (time_now_usec - att.timestamp < 1_s) && (att.timestamp > 0); - } - - vehicle_local_position_s lpos{}; - - if (_vehicle_local_position_sub.copy(&lpos)) { - lpos_valid = (time_now_usec - lpos.timestamp < 1_s) && (lpos.timestamp > 0) && lpos.v_xy_valid; - } - - // update wind and airspeed estimator - _wind_estimator.update(time_now_usec); - - if (lpos_valid && att_valid) { - - Vector3f vI(lpos.vx, lpos.vy, lpos.vz); - Quatf q(att.q); - - // sideslip fusion - _wind_estimator.fuse_beta(time_now_usec, vI, q); - - // additionally, for airspeed fusion we need to have recent measurements - airspeed_s airspeed{}; - - if (_airspeed_sub.update(&airspeed)) { - if ((time_now_usec - airspeed.timestamp < 1_s) && (airspeed.timestamp > 0)) { - const Vector3f vel_var{Dcmf(q) *Vector3f{lpos.evh, lpos.evh, lpos.evv}}; - - // airspeed fusion - _wind_estimator.fuse_airspeed(time_now_usec, airspeed.true_airspeed_m_s, vI, Vector2f{vel_var(0), vel_var(1)}); - } - } - - // if we fused either airspeed or sideslip we publish a wind_estimate message - wind_estimate_s wind_est = {}; - - wind_est.timestamp = time_now_usec; - float wind[2]; - _wind_estimator.get_wind(wind); - wind_est.windspeed_north = wind[0]; - wind_est.windspeed_east = wind[1]; - float wind_cov[2]; - _wind_estimator.get_wind_var(wind_cov); - wind_est.variance_north = wind_cov[0]; - wind_est.variance_east = wind_cov[1]; - wind_est.tas_innov = _wind_estimator.get_tas_innov(); - wind_est.tas_innov_var = _wind_estimator.get_tas_innov_var(); - wind_est.beta_innov = _wind_estimator.get_beta_innov(); - wind_est.beta_innov_var = _wind_estimator.get_beta_innov_var(); - wind_est.tas_scale = _wind_estimator.get_tas_scale(); - - orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub, &wind_est, &_instance, ORB_PRIO_DEFAULT); - } - - perf_end(_perf_elapsed); - - if (should_exit()) { - exit_and_cleanup(); - } -} - -void WindEstimatorModule::update_params() -{ - updateParams(); - - // update wind & airspeed scale estimator parameters - _wind_estimator.set_wind_p_noise(_param_west_w_p_noise.get()); - _wind_estimator.set_tas_scale_p_noise(_param_west_sc_p_noise.get()); - _wind_estimator.set_tas_noise(_param_west_tas_noise.get()); - _wind_estimator.set_beta_noise(_param_west_beta_noise.get()); - _wind_estimator.set_tas_gate(_param_west_tas_gate.get()); - _wind_estimator.set_beta_gate(_param_west_beta_gate.get()); -} - -int WindEstimatorModule::custom_command(int argc, char *argv[]) -{ - if (!is_running()) { - int ret = WindEstimatorModule::task_spawn(argc, argv); - - if (ret) { - return ret; - } - } - - return print_usage("unknown command"); -} - -int WindEstimatorModule::print_usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description -This module runs a combined wind and airspeed scale factor estimator. -If provided the vehicle NED speed and attitude it can estimate the horizontal wind components based on a zero -sideslip assumption. This makes the estimator only suitable for fixed wing vehicles. -If provided an airspeed measurement this module also estimates an airspeed scale factor based on the following model: -measured_airspeed = scale_factor * real_airspeed. - -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("wind_estimator", "estimator"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - - return 0; -} - -int WindEstimatorModule::print_status() -{ - perf_print_counter(_perf_elapsed); - perf_print_counter(_perf_interval); - - if (_instance > -1) { - uORB::SubscriptionData est{ORB_ID(wind_estimate), (uint8_t)_instance}; - est.update(); - - print_message(est.get()); - } else { - PX4_INFO("Running, but never published"); - } - - return 0; -} - -extern "C" __EXPORT int wind_estimator_main(int argc, char *argv[]); - -int -wind_estimator_main(int argc, char *argv[]) -{ - return WindEstimatorModule::main(argc, argv); -} diff --git a/src/modules/wind_estimator/wind_estimator_params.c b/src/modules/wind_estimator/wind_estimator_params.c deleted file mode 100644 index eb4f71b0f9..0000000000 --- a/src/modules/wind_estimator/wind_estimator_params.c +++ /dev/null @@ -1,71 +0,0 @@ -/** - * Enable Wind estimator - * - * @boolean - * @reboot_required true - * @group Wind Estimator - */ -PARAM_DEFINE_INT32(WEST_EN, 0); - -/** - * Wind estimator wind process noise. - * - * @min 0 - * @max 1 - * @unit m/s/s - * @group Wind Estimator - */ -PARAM_DEFINE_FLOAT(WEST_W_P_NOISE, 0.1f); - -/** - * Wind estimator true airspeed scale process noise. - * - * @min 0 - * @max 0.1 - * @group Wind Estimator - */ -PARAM_DEFINE_FLOAT(WEST_SC_P_NOISE, 0.0001); - -/** - * Wind estimator true airspeed measurement noise. - * - * @min 0 - * @max 4 - * @unit m/s - * @group Wind Estimator - */ -PARAM_DEFINE_FLOAT(WEST_TAS_NOISE, 1.4); - -/** - * Wind estimator sideslip measurement noise. - * - * @min 0 - * @max 1 - * @unit rad - * @group Wind Estimator - */ -PARAM_DEFINE_FLOAT(WEST_BETA_NOISE, 0.3); - -/** - * Gate size for true airspeed fusion. - * - * Sets the number of standard deviations used by the innovation consistency test. - * - * @min 1 - * @max 5 - * @unit SD - * @group Wind Estimator - */ -PARAM_DEFINE_INT32(WEST_TAS_GATE, 3); - -/** - * Gate size for true sideslip fusion. - * - * Sets the number of standard deviations used by the innovation consistency test. - * - * @min 1 - * @max 5 - * @unit SD - * @group Wind Estimator - */ -PARAM_DEFINE_INT32(WEST_BETA_GATE, 1);