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};