From d532bc955509db5c6a67bfbdb037e327fe57b2a3 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 15 Dec 2020 16:15:24 +0100 Subject: [PATCH] HTE: only run in air the _landed flag only is not enough to tell if the drone is still touching the ground, so an additional check based on the altitude AGL is required. to have this in_air detection correctly updated, the module needs to be scheduled on the vehicle_local_position message as no setpoint is produced in non-assisted modes. --- .../MulticopterHoverThrustEstimator.cpp | 67 +++++++++++-------- .../MulticopterHoverThrustEstimator.hpp | 4 +- 2 files changed, 40 insertions(+), 31 deletions(-) diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index 6851e9f41a..310d799869 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -59,7 +59,7 @@ MulticopterHoverThrustEstimator::~MulticopterHoverThrustEstimator() bool MulticopterHoverThrustEstimator::init() { - if (!_vehicle_local_position_setpoint_sub.registerCallback()) { + if (!_vehicle_local_position_sub.registerCallback()) { PX4_ERR("vehicle_local_position_setpoint callback registration failed!"); return false; } @@ -91,13 +91,43 @@ void MulticopterHoverThrustEstimator::updateParams() void MulticopterHoverThrustEstimator::Run() { if (should_exit()) { - _vehicle_local_position_setpoint_sub.unregisterCallback(); + _vehicle_local_position_sub.unregisterCallback(); exit_and_cleanup(); return; } - // new local position estimate and setpoint needed every iteration - if (!_vehicle_local_pos_sub.updated() || !_vehicle_local_position_setpoint_sub.updated()) { + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + + if (_landed) { + _in_air = false; + } + } + } + + if (!_vehicle_local_position_sub.updated()) { + return; + } + + vehicle_local_position_s local_pos{}; + + if (_vehicle_local_position_sub.copy(&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 + if (local_pos.dist_bottom_valid) { + if (!_landed && (local_pos.dist_bottom > 1.f)) { + _in_air = true; + } + } + } + + // new local position setpoint needed every iteration + if (!_vehicle_local_position_setpoint_sub.updated()) { return; } @@ -113,14 +143,6 @@ void MulticopterHoverThrustEstimator::Run() perf_begin(_cycle_perf); - if (_vehicle_land_detected_sub.updated()) { - vehicle_land_detected_s vehicle_land_detected; - - if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { - _landed = vehicle_land_detected.landed; - } - } - if (_vehicle_status_sub.updated()) { vehicle_status_s vehicle_status; @@ -129,36 +151,23 @@ void MulticopterHoverThrustEstimator::Run() } } - // always copy latest position estimate - vehicle_local_position_s local_pos{}; - - if (_vehicle_local_pos_sub.copy(&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 - if (local_pos.dist_bottom_valid) { - _in_air = local_pos.dist_bottom > 1.f; - } - } - const float dt = (local_pos.timestamp - _timestamp_last) * 1e-6f; _timestamp_last = local_pos.timestamp; - if (_armed && !_landed && (dt > 0.001f) && (dt < 1.f) && PX4_ISFINITE(local_pos.az)) { + if (_armed && _in_air && (dt > 0.001f) && (dt < 1.f) && PX4_ISFINITE(local_pos.az)) { _hover_thrust_ekf.predict(dt); vehicle_local_position_setpoint_s local_pos_sp; - if (_vehicle_local_position_setpoint_sub.update(&local_pos_sp)) { + if (_vehicle_local_position_setpoint_sub.copy(&local_pos_sp)) { if (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) - ZeroOrderHoverThrustEkf::status status; + ZeroOrderHoverThrustEkf::status status{}; _hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2], status); - const bool valid = _in_air && (status.hover_thrust_var < 0.001f) && (status.innov_test_ratio < 1.f); + const bool valid = (status.hover_thrust_var < 0.001f) && (status.innov_test_ratio < 1.f); _valid_hysteresis.set_state_and_update(valid, local_pos.timestamp); _valid = _valid_hysteresis.get_state(); diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp index 0e6853d4c5..cad76b33f9 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp @@ -95,12 +95,12 @@ private: 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::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; 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)}; + uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; hrt_abstime _timestamp_last{0};